cframe.c

Go to the documentation of this file.

#include <config.h>

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>

#define DEBUG_LEVEL 1
#include "debug.h"
#include <cartcomm/cframe.h>
#include <cartcomm/nmea.h>
#include <cartcomm/comm.h>

// Provided by cframe_parser.c
int parse_msg(cframe_t *cf, char *msg);

//#define SANITY_CHECK


static unsigned char compute_checksum(char *msg, int start, int stop) {
    int i;
    unsigned int checksum = 0;

    for(i = start; i < stop; i++) {
        checksum += (unsigned int) msg[i];
    }

    checksum %= 255; // Gag.. Vomit.. Gavomit!
    return checksum;
}


int cartcomm_encode_msg(cframe_t *cf, char *outMsg) {
    char field[80];
    char nmea[NMEA_MAXSIZE];
    char msg[MAX_MSG_SIZE];
    int len;

    strcpy(msg, "");

#define APPEND(fmt, val) sprintf(field, fmt, val); strcat(msg, field);

    // Header
    APPEND("VehMsgCount=%ld,", cf->seqno);
    APPEND("TimeStamp=%ld,", cf->timestamp);

    // Steering
    APPEND("CmdTurnRadius=%.5f,", cf->cmd_irad);
    APPEND("StatusTurnRadius=%.5f,", cf->cur_irad);
    APPEND("StatusSteeringPosLimitError=%s,", cf->steer_limit_error);

    // Speed
    APPEND("CmdVehSpeed=%.2f,", cf->cmd_speed);
    APPEND("StatusVehSpeed=%.2f,", cf->cur_speed);
    APPEND("StatusPercentThrottle=%.2f,", cf->cur_throttle);
    APPEND("StatusVehSpeedLimitError=%s,", cf->speed_limit_error);

    // GPS
    deg_to_nmea(cf->gps_lat, nmea, sizeof(nmea));
    APPEND("GPSLat=%s,", nmea);
    deg_to_nmea(cf->gps_lon, nmea, sizeof(nmea));
    APPEND("GPSLon=%s,", nmea);
    APPEND("GPSSpdE=%.3f,", cf->gps_speed_east);
    APPEND("GPSSpdN=%.3f,", cf->gps_speed_north);
//  APPEND("GPSEstPositionError=%.2f,", cf->GPSEpe);
    APPEND("GPSEstimatedPositionError=%.2f,", cf->gps_epe);
    APPEND("GPSStatus=%s,", cf->gps_status);

    // Status of the driver/cart connection
    APPEND("StatusComm=%s,", cf->status);

    // Desired system mode
    APPEND("CmdSystemMode=%s,", cf->cmd_mode);

    // System mode
    APPEND("StatusSystemMode=%s,", cf->cur_mode);

    // Error code
    APPEND("StatusErrorCode=%d,", cf->error_code);

    // Message length (header and data) in bytes
    len = strlen(msg) + 11;

    // Why oh why does MsgLength include itself?!
    if(len + 1 <= 9) {
        APPEND("MsgLength=%1d,", len + 1);
    } else if(len + 2 <= 99) {
        APPEND("MsgLength=%2d,", len + 2);
    } else if(len + 3 <= 999) {
        APPEND("MsgLength=%3d,", len + 3);
    } else if(len + 4 <= 9999) {
        APPEND("MsgLength=%4d,", len + 4);
    }

    // Checksum of header and data
    sprintf(outMsg, "{%s%02x}", msg, compute_checksum(msg, 0, strlen(msg)));

    DEBUG3(">>> %s", outMsg);

#ifdef SANITY_CHECK
    cframe_t tcf;
    int result;
    result = cartcomm_decode_msg(&tcf, outMsg);
    ASSERT_EQUAL(result, 0);
#endif

    return 0;
}


int cartcomm_decode_msg(cframe_t *cf, char *inMsg) {
    int msgLen;
    int checksum;

    DEBUG3("<<< %s", inMsg);

    parse_msg(cf, inMsg);

//    print_cframe(stderr, cf);

    // don't count the guards or the checksum
    msgLen = strlen(inMsg) - 4;

    if(cf->msg_len != msgLen) {
        DEBUG2("wrong message length (theirs: %d, ours: %d)",
               cf->msg_len, msgLen);
//        return -1;
    }

    // truncate to appropriate message for checksumming
    checksum = compute_checksum(inMsg, 1, strlen(inMsg) - 3);

    if(cf->checksum != checksum) {
        DEBUG2("checksum failed  (theirs: %d, ours: %d)",
               cf->checksum, checksum);
//        return -1;
    }

    return 0;
}


void print_cframe(FILE *file, cframe_t *cf) {
#define PRINT(...) fprintf(file, "\t" __VA_ARGS__); fputc('\n', file);

    fprintf(file, "{\n");

    PRINT("seqno = %ld\t", cf->seqno);
    PRINT("timestamp = %ld\t", cf->timestamp);

    PRINT("cmd_irad = %.5f\t", cf->cmd_irad);
    PRINT("cur_irad = %.5f\t", cf->cur_irad);
    PRINT("steer_limit_error = %s\t", cf->steer_limit_error);

    PRINT("cmd_speed = %.2f\t", cf->cmd_speed);
    PRINT("cur_speed = %.2f\t", cf->cur_speed);
    PRINT("cur_throttle = %.2f\t", cf->cur_throttle);
    PRINT("speed_limit_error = %s\t", cf->speed_limit_error);

    PRINT("gps_lat = %.6f\t", cf->gps_lat);
    PRINT("gps_lon = %.6f\t", cf->gps_lon);
    PRINT("gps_speed_east = %.3f\t", cf->gps_speed_east);
    PRINT("gps_speed_north = %.3f\t", cf->gps_speed_north);
    PRINT("gps_epe = %.2f\t", cf->gps_epe);
    PRINT("gps_status = %s\t", cf->gps_status);

    PRINT("status = %s\t", cf->status);
    PRINT("cmd_mode = %s\t", cf->cmd_mode);
    PRINT("cur_mode = %s\t", cf->cur_mode);
    PRINT("error_code = %d\t", cf->error_code);

    fprintf(file, "}\n");
}

---