DJI NAZA-M controller (remote controller side) see: https://developer.mbed.org/users/okini3939/notebook/drone/
Dependencies: NECnfc SpiOLED USBHost mbed
Diff: naza.cpp
- Revision:
- 0:9f11e7a30865
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/naza.cpp Thu May 19 09:03:44 2016 +0000 @@ -0,0 +1,70 @@ +#include "mbed.h" +#include "Naza.h" +#include "drone.h" +#include <math.h> + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +extern struct Status stat; + +void parseGps (uint8_t *buf) { + struct Naza_Gps *gps = (struct Naza_Gps *)buf; + + if (gps->mask != 0) return; // mask +/* +for (int i = 0; i < 60; i ++) { + printf(" %02x", buf[i]); +} +printf("\r\n"); +*/ + 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); +} + +void parseCompass (uint8_t *buf) { + struct Naza_Compass *compass = (struct Naza_Compass *)buf; + + 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; + + if (stat.compass_x >= 0) { + if (stat.compass_y <= 0) { + stat.compass = -stat.compass; + } else { + stat.compass = 360 - stat.compass; + } + } else { + stat.compass = 180 - stat.compass; + } +}