Suga koubou / Mbed 2 deprecated Drone_ground

DJI NAZA-M controller (remote controller side) see: https://developer.mbed.org/users/okini3939/notebook/drone/

Dependencies:   NECnfc SpiOLED USBHost mbed

naza.cpp@1:d83f8332ebfe, 2016-05-19 (annotated)

Committer:
okini3939
Date:
Thu May 19 09:05:02 2016 +0000
Revision:
1:d83f8332ebfe
Parent:
0:9f11e7a30865 
1st build;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
okini3939 0:9f11e7a30865 1 #include "mbed.h"
okini3939 0:9f11e7a30865 2 #include "Naza.h"
okini3939 0:9f11e7a30865 3 #include "drone.h"
okini3939 0:9f11e7a30865 4 #include <math.h>
okini3939 0:9f11e7a30865 5
okini3939 0:9f11e7a30865 6 #ifndef M_PI
okini3939 0:9f11e7a30865 7 #define M_PI 3.14159265358979323846
okini3939 0:9f11e7a30865 8 #endif
okini3939 0:9f11e7a30865 9
okini3939 0:9f11e7a30865 10 extern struct Status stat;
okini3939 0:9f11e7a30865 11
okini3939 0:9f11e7a30865 12 void parseGps (uint8_t *buf) {
okini3939 0:9f11e7a30865 13 struct Naza_Gps *gps = (struct Naza_Gps *)buf;
okini3939 0:9f11e7a30865 14
okini3939 0:9f11e7a30865 15 if (gps->mask != 0) return; // mask
okini3939 0:9f11e7a30865 16 /*
okini3939 0:9f11e7a30865 17 for (int i = 0; i < 60; i ++) {
okini3939 0:9f11e7a30865 18 printf(" %02x", buf[i]);
okini3939 0:9f11e7a30865 19 }
okini3939 0:9f11e7a30865 20 printf("\r\n");
okini3939 0:9f11e7a30865 21 */
okini3939 0:9f11e7a30865 22 if (gps->datetime) {
okini3939 0:9f11e7a30865 23 stat.gps_date = 20000000 + ((gps->datetime >> 25) & 0x3f) * 10000 + ((gps->datetime >> 21) & 0x0f) * 100 + ((gps->datetime >> 16) & 0x1f);
okini3939 0:9f11e7a30865 24 stat.gps_time = ((gps->datetime >> 12) & 0x0f) * 10000 + ((gps->datetime >> 6) & 0x3f) * 100 + (gps->datetime & 0x3f);
okini3939 0:9f11e7a30865 25 }
okini3939 0:9f11e7a30865 26
okini3939 0:9f11e7a30865 27 stat.gps_lng = gps->longitude;
okini3939 0:9f11e7a30865 28 stat.gps_lat = gps->latitude;
okini3939 0:9f11e7a30865 29 stat.gps_h = gps->altitude;
okini3939 0:9f11e7a30865 30 stat.gps_sat = gps->satellites;
okini3939 0:9f11e7a30865 31 stat.gps_type = gps->fix_type;
okini3939 0:9f11e7a30865 32 stat.gps_flg = gps->flags;
okini3939 0:9f11e7a30865 33
okini3939 0:9f11e7a30865 34 if (stat.gps_lng && (stat.gps_lng < 1225601000 || stat.gps_lng > 1535911000)) { // yonaguni , minamitori
okini3939 0:9f11e7a30865 35 stat.gps_lng = 0;
okini3939 0:9f11e7a30865 36 stat.gps_lost ++;
okini3939 0:9f11e7a30865 37 }
okini3939 0:9f11e7a30865 38 if (stat.gps_lat && (stat.gps_lat < 202531000 || stat.gps_lat > 453326000)) { // okinotori , etorofu
okini3939 0:9f11e7a30865 39 stat.gps_lat = 0;
okini3939 0:9f11e7a30865 40 stat.gps_lost ++;
okini3939 0:9f11e7a30865 41 }
okini3939 0:9f11e7a30865 42
okini3939 0:9f11e7a30865 43 if (stat.gps_type == 2 || stat.gps_type == 3) {
okini3939 0:9f11e7a30865 44 stat.gps_lost = 0;
okini3939 0:9f11e7a30865 45 } else {
okini3939 0:9f11e7a30865 46 stat.gps_lost ++;
okini3939 0:9f11e7a30865 47 }
okini3939 0:9f11e7a30865 48 // printf("%08d %06d lat=%9d lng=%10d h=%4d sat=%d type=%d flg=%d\r\n",
okini3939 0:9f11e7a30865 49 // stat.gps_date, stat.gps_time, stat.gps_lat, stat.gps_lng, stat.gps_h, stat.gps_sat, stat.gps_type, stat.gps_flg);
okini3939 0:9f11e7a30865 50 }
okini3939 0:9f11e7a30865 51
okini3939 0:9f11e7a30865 52 void parseCompass (uint8_t *buf) {
okini3939 0:9f11e7a30865 53 struct Naza_Compass *compass = (struct Naza_Compass *)buf;
okini3939 0:9f11e7a30865 54
okini3939 0:9f11e7a30865 55 stat.compass_x = compass->x;
okini3939 0:9f11e7a30865 56 stat.compass_y = compass->y;
okini3939 0:9f11e7a30865 57 stat.compass_z = compass->z;
okini3939 0:9f11e7a30865 58 float mg = sqrtf(stat.compass_x * stat.compass_x + stat.compass_y * stat.compass_y);
okini3939 0:9f11e7a30865 59 stat.compass = atanf((stat.compass_y / mg) / (stat.compass_x / mg)) * 180.0 / M_PI;
okini3939 0:9f11e7a30865 60
okini3939 0:9f11e7a30865 61 if (stat.compass_x >= 0) {
okini3939 0:9f11e7a30865 62 if (stat.compass_y <= 0) {
okini3939 0:9f11e7a30865 63 stat.compass = -stat.compass;
okini3939 0:9f11e7a30865 64 } else {
okini3939 0:9f11e7a30865 65 stat.compass = 360 - stat.compass;
okini3939 0:9f11e7a30865 66 }
okini3939 0:9f11e7a30865 67 } else {
okini3939 0:9f11e7a30865 68 stat.compass = 180 - stat.compass;
okini3939 0:9f11e7a30865 69 }
okini3939 0:9f11e7a30865 70 }