naza.cpp

00001 #include "mbed.h"
00002 #include "Naza.h"
00003 #include "drone.h"
00004 #include <math.h>
00005 
00006 #ifndef M_PI
00007 #define M_PI 3.14159265358979323846
00008 #endif
00009 
00010 extern struct Status stat;
00011 
00012 void parseGps (uint8_t *buf) {
00013         struct Naza_Gps *gps = (struct Naza_Gps *)buf;
00014 
00015         if (gps->mask != 0) return; // mask
00016 /*
00017 for (int i = 0; i < 60; i ++) {
00018     printf(" %02x", buf[i]);
00019 }
00020 printf("\r\n");
00021 */
00022         if (gps->datetime) {
00023             stat.gps_date = 20000000 + ((gps->datetime >> 25) & 0x3f) * 10000 + ((gps->datetime >> 21) & 0x0f) * 100 + ((gps->datetime >> 16) & 0x1f);
00024             stat.gps_time = ((gps->datetime >> 12) & 0x0f) * 10000 + ((gps->datetime >> 6) & 0x3f) * 100 + (gps->datetime & 0x3f);
00025         }
00026 
00027         stat.gps_lng = gps->longitude;
00028         stat.gps_lat = gps->latitude;
00029         stat.gps_h    = gps->altitude;
00030         stat.gps_sat  = gps->satellites;
00031         stat.gps_type = gps->fix_type;
00032         stat.gps_flg  = gps->flags;
00033 
00034         if (stat.gps_lng && (stat.gps_lng < 1225601000 || stat.gps_lng > 1535911000)) { // yonaguni , minamitori
00035             stat.gps_lng = 0;
00036             stat.gps_lost ++;
00037         }
00038         if (stat.gps_lat && (stat.gps_lat <  202531000 || stat.gps_lat >  453326000)) { // okinotori , etorofu
00039             stat.gps_lat = 0;
00040             stat.gps_lost ++;
00041         }
00042 
00043         if (stat.gps_type == 2 || stat.gps_type == 3) {
00044             stat.gps_lost = 0;
00045         } else {
00046             stat.gps_lost ++;
00047         }
00048 //    printf("%08d %06d lat=%9d lng=%10d h=%4d sat=%d type=%d flg=%d\r\n",
00049 //        stat.gps_date, stat.gps_time, stat.gps_lat, stat.gps_lng, stat.gps_h, stat.gps_sat, stat.gps_type, stat.gps_flg);
00050 }
00051 
00052 void parseCompass (uint8_t *buf) {
00053         struct Naza_Compass *compass = (struct Naza_Compass *)buf;
00054 
00055         stat.compass_x = compass->x;
00056         stat.compass_y = compass->y;
00057         stat.compass_z = compass->z;
00058         float mg = sqrtf(stat.compass_x * stat.compass_x + stat.compass_y * stat.compass_y);
00059         stat.compass   = atanf((stat.compass_y / mg) / (stat.compass_x / mg)) * 180.0 / M_PI;
00060 
00061     if (stat.compass_x >= 0) {
00062         if (stat.compass_y <= 0) {
00063             stat.compass = -stat.compass;
00064         } else {
00065             stat.compass = 360 - stat.compass;
00066         }
00067     } else {
00068         stat.compass = 180 - stat.compass;
00069     }
00070 }