File content as of revision 1:32cd1cf5d5b1:

#include "mbed.h"
#include "Naza.h"
#include "drone.h"
#include "math.h"

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

CAN can(p30, p29); // can_rx2, can_tx2

static uint8_t can_buf[80];
static int can_count = 0, can_id = 0, can_len = 0;

extern struct GroundData send_data;
extern struct Status stat;
extern DigitalOut led3;

void parseCan (int id, int len) {
    int i;

    switch (id) {
    case 0x1003: // gps
        if (len != 58) break;
        struct Naza_Gps *gps = (struct Naza_Gps *)can_buf;

        memcpy(&send_data.gps, can_buf, len);
        if (gps->mask != 0) break; // mask

        if (gps->datetime) {
            stat.gps_date = 20000000 + ((gps->datetime >> 25) & 0x3f) * 10000 + ((gps->datetime >> 21) & 0x0f) * 100 + ((gps->datetime >> 16) & 0x1f);
            stat.gps_time = ((gps->datetime >> 12) & 0x0f) * 10000 + ((gps->datetime >> 6) & 0x3f) * 100 + (gps->datetime & 0x3f);
        }

        stat.gps_lng = gps->longitude;
        stat.gps_lat = gps->latitude;
        stat.gps_h    = gps->altitude;
        stat.gps_sat  = gps->satellites;
        stat.gps_type = gps->fix_type;
        stat.gps_flg  = gps->flags;

        if (stat.gps_lng && (stat.gps_lng < 1225601000 || stat.gps_lng > 1535911000)) { // yonaguni , minamitori
            stat.gps_lng = 0;
            stat.gps_lost ++;
        }
        if (stat.gps_lat && (stat.gps_lat <  202531000 || stat.gps_lat >  453326000)) { // okinotori , etorofu
            stat.gps_lat = 0;
            stat.gps_lost ++;
        }

        if (stat.gps_type == 2 || stat.gps_type == 3) {
            stat.gps_lost = 0;
        } else {
            stat.gps_lost ++;
        }
//    printf("%08d %06d lat=%9d lng=%10d h=%4d sat=%d type=%d flg=%d\r\n",
//        stat.gps_date, stat.gps_time, stat.gps_lat, stat.gps_lng, stat.gps_h, stat.gps_sat, stat.gps_type, stat.gps_flg);
        break;

    case 0x1004: // compass
        if (len != 6) break;
        struct Naza_Compass *compass = (struct Naza_Compass *)can_buf;

        memcpy(&send_data.compass, can_buf, len);
        stat.compass_x = compass->x;
        stat.compass_y = compass->y;
        stat.compass_z = compass->z;
        float mg = sqrtf(stat.compass_x * stat.compass_x + stat.compass_y * stat.compass_y);
        stat.compass   = atanf((stat.compass_y / mg) / (stat.compass_x / mg)) * 180.0 / M_PI;
        break;

    case 0x0921: // gps version
    case 0x0922: // pmu heart beat
        break;

    default:
            printf("parse id=%04x %d, ", id, len);
            for (i = 0; i < can_len; i ++) {
                printf("%02x ", can_buf[i]);
            }
            printf("\r\n");
        break;
    }
}

void isrCan () {
    int i;
    CANMessage msg;

    __disable_irq();
    if (can.read(msg)) {
        __enable_irq();
        // can
        switch (msg.id) {
        case 0x07f8: // gps
        case 0x0118: // compass
        case 0x01fe: // battery
            if (can_id == 0 && msg.data[0] == 0x55 && msg.data[1] == 0xaa && msg.data[2] == 0x55 && msg.data[3] == 0xaa) {
                // header
                can_len = (msg.data[7] << 8) | msg.data[6];
                if (can_len < 0 || can_len >= sizeof(can_buf)) break;
                can_id  = (msg.data[5] << 8) | msg.data[4];
                can_count = 0;
            } else
            if (can_id) {
                // payload
                for (i = 0; i < msg.len; i ++) {
                    if (can_count < sizeof(can_buf)) {
                        can_buf[can_count] = msg.data[i];
                    }
                    can_count ++;
                }
                if (can_count >= can_len + 4) { // + footer
                    // done
                    if (can_buf[can_len] == 0x66 && can_buf[can_len + 1] == 0xcc && can_buf[can_len + 2] == 0x66 && can_buf[can_len + 3] == 0xcc) {
                        parseCan(can_id, can_len);
                        led3 = !led3;
                    }
                    can_id = 0;
                }
            }
            break;

        default:
            printf("CAN id=%04x, ", msg.id);
            for (i = 0; i < msg.len; i ++) {
                printf("%02x ", msg.data[i]);
            }
            printf("\r\n");
            break;
        }
    }
    __enable_irq();
}

void initCan () {
    can.frequency(1000000);
    can.attach(&isrCan, CAN::RxIrq);
}