[gps_datalink] update small message protocol

conf/airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml

@@ -161,13 +161,6 @@
     <define name="USE_GPS_ALT" value="1"/>
   </section>
 
-  <section name="GPS" prefix="GPS_">
-    <define name="USE_DATALINK_SMALL" value="1"/>
-    <define name="LOCAL_ECEF_ORIGIN_X" value="392433200"/>
-    <define name="LOCAL_ECEF_ORIGIN_Y" value="30036200"/>
-    <define name="LOCAL_ECEF_ORIGIN_Z" value="500219700"/>
-  </section>
-
  <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
    <define name="HOVER_KP" value="276"/>
    <define name="HOVER_KD" value="455"/>
@@ -183,8 +176,6 @@
     <define name="H_Z" value="0.92060036"/>
     <define name="USE_GPS_HEADING" value="1"/>
   </section>
-<define name="USE_GPS" value="TRUE"/>
- <define name="USE_GPS_HEADING" value="TRUE"/>
 
  <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
    <define name="PGAIN" value="290"/>

conf/airframes/LS/ls_ladybird_lisa_s_bluegiga_small_gps_messages.xml

@@ -173,13 +173,6 @@
     <define name="USE_GPS_ALT" value="1"/>
   </section>
 
-  <section name="GPS" prefix="GPS_">
-    <define name="USE_DATALINK_SMALL" value="1"/>
-    <define name="LOCAL_ECEF_ORIGIN_X" value="392433200"/>
-    <define name="LOCAL_ECEF_ORIGIN_Y" value="30036200"/>
-    <define name="LOCAL_ECEF_ORIGIN_Z" value="500219700"/>
-  </section>
-
  <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
    <define name="HOVER_KP" value="276"/>
    <define name="HOVER_KD" value="455"/>

conf/airframes/LS/ls_quadrotor_bebop_small_gps_messages.xml

@@ -102,13 +102,6 @@
     <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
   </section>
 
-  <section name="GPS" prefix="GPS_">
-    <define name="USE_DATALINK_SMALL" value="1"/>
-    <define name="LOCAL_ECEF_ORIGIN_X" value="392433200"/>
-    <define name="LOCAL_ECEF_ORIGIN_Y" value="30036200"/>
-    <define name="LOCAL_ECEF_ORIGIN_Z" value="500219700"/>
-  </section>
-
   <section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
     <!-- setpoints -->
     <define name="SP_MAX_P" value="140" unit="deg/s"/>

conf/messages.xml

@@ -856,7 +856,6 @@
 
   <message name="BLUEGIGA" id="106">
     <field name="data_rate" type="uint32" unit="bytes/s"/>
-    <field name="last_msg" type="uint8[]"/>
   </message>
 
   <!--107 is free -->
@@ -2479,8 +2478,9 @@
   <message name="REMOTE_GPS_SMALL" id="54" link="broadcasted">
     <field name="ac_id"   type="uint8"/>
     <field name="numsv"   type="uint8"/>
-    <field name="pos_xyz"  type="uint32" unit="cm">bits 31-22 x position in cm : bits 21-12 y position in cm : bits 11-2 z position in cm : bits 1 and 0 are free</field>
+    <field name="pos_xyz"  type="uint32" unit="cm">bits 31-21 x position in cm : bits 20-10 y position in cm : bits 9-0 z position in cm</field>
-    <field name="speed_xy"  type="uint32" unit="cm/s">bits 31-22 speed x in cm/s : bits 21-12 speed y in cm/s : bits 11-2 heading in rad*1e2 : bits 1 and 0 are free</field>
+    <field name="speed_xyz"  type="uint32" unit="cm/s">bits 31-21 speed x in cm/s : bits 20-10 speed y in cm/s : bits 9-0 speed z in cm/s</field>
+    <field name="heading"   type="int16" unit="rad*1e4"/>
   </message>
 
   <message name="REMOTE_GPS" id="55" link="broadcasted">

sw/airborne/firmwares/rotorcraft/datalink.c

@@ -90,7 +90,7 @@ void dl_parse_msg(void)
     }
     break;
 
-#if defined USE_NAVIGATION
+#ifdef USE_NAVIGATION
     case DL_BLOCK : {
       if (DL_BLOCK_ac_id(dl_buffer) != AC_ID) { break; }
       nav_goto_block(DL_BLOCK_block_id(dl_buffer));
@@ -138,21 +138,21 @@ void dl_parse_msg(void)
       }
       break;
 #endif // RADIO_CONTROL_TYPE_DATALINK
-#if defined GPS_DATALINK
+#if USE_GPS
-#ifdef GPS_USE_DATALINK_SMALL
+#ifdef GPS_DATALINK
-    case DL_REMOTE_GPS_SMALL :
+    case DL_REMOTE_GPS_SMALL : {
       // Check if the GPS is for this AC
-      if (DL_REMOTE_GPS_SMALL_ac_id(dl_buffer) != AC_ID) {
+      if (DL_REMOTE_GPS_SMALL_ac_id(dl_buffer) != AC_ID) { break; }
-        break;
-      }
 
       parse_gps_datalink_small(
         DL_REMOTE_GPS_SMALL_numsv(dl_buffer),
         DL_REMOTE_GPS_SMALL_pos_xyz(dl_buffer),
-        DL_REMOTE_GPS_SMALL_speed_xy(dl_buffer));
+        DL_REMOTE_GPS_SMALL_speed_xyz(dl_buffer),
-      break;
+        DL_REMOTE_GPS_SMALL_heading(dl_buffer));
-#endif
+    }
-    case DL_REMOTE_GPS :
+    break;
+
+    case DL_REMOTE_GPS : {
       // Check if the GPS is for this AC
       if (DL_REMOTE_GPS_ac_id(dl_buffer) != AC_ID) { break; }
 
@@ -171,10 +171,11 @@ void dl_parse_msg(void)
         DL_REMOTE_GPS_ecef_zd(dl_buffer),
         DL_REMOTE_GPS_tow(dl_buffer),
         DL_REMOTE_GPS_course(dl_buffer));
-      break;
+    }
-#endif
+    break;
-#if USE_GPS
+#endif // GPS_DATALINK
-    case DL_GPS_INJECT :
+
+    case DL_GPS_INJECT : {
       // Check if the GPS is for this AC
       if (DL_GPS_INJECT_ac_id(dl_buffer) != AC_ID) { break; }
 
@@ -183,9 +184,10 @@ void dl_parse_msg(void)
         DL_GPS_INJECT_packet_id(dl_buffer),
         DL_GPS_INJECT_data_length(dl_buffer),
         DL_GPS_INJECT_data(dl_buffer)
-        );
+      );
-      break;
+    }
-#endif
+    break;
+#endif  // USE_GPS
 
     case DL_GUIDED_SETPOINT_NED:
       if (DL_GUIDED_SETPOINT_NED_ac_id(dl_buffer) != AC_ID) { break; }

sw/airborne/subsystems/gps/gps_datalink.c

@@ -27,35 +27,19 @@
  * GPS structure to the values received.
  */
 
+#include "messages.h"
+#include "generated/airframe.h"           // AC_ID
+#include "generated/flight_plan.h"        // reference lla NAV_XXX0
+#include "subsystems/datalink/downlink.h"
+
 #include "subsystems/gps.h"
 #include "subsystems/abi.h"
 
-// #include <stdio.h> 
+// #include <stdio.h>
-
-#if GPS_USE_DATALINK_SMALL
-#ifndef GPS_LOCAL_ECEF_ORIGIN_X
-#error Local x coordinate in ECEF of the remote GPS required
-#endif
-
-#ifndef GPS_LOCAL_ECEF_ORIGIN_Y
-#error Local y coordinate in ECEF of the remote GPS required
-#endif
-
-#ifndef GPS_LOCAL_ECEF_ORIGIN_Z
-#error Local z coordinate in ECEF of the remote GPS required
-#endif
-
-struct EcefCoor_i tracking_ecef;
-
-struct LtpDef_i tracking_ltp;
 
+struct LtpDef_i ltp_def;
 struct EnuCoor_i enu_pos, enu_speed;
 
-struct EcefCoor_i ecef_pos, ecef_vel;
-
-struct LlaCoor_i lla_pos;
-#endif
-
 bool_t gps_available;   ///< Is set to TRUE when a new REMOTE_GPS packet is received and parsed
 
 /** GPS initialization */
@@ -66,70 +50,68 @@ void gps_impl_init(void)
   gps.gspeed = 700; // To enable course setting
   gps.cacc = 0; // To enable course setting
 
-#if GPS_USE_DATALINK_SMALL
+  struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
-  tracking_ecef.x = GPS_LOCAL_ECEF_ORIGIN_X;
+  llh_nav0.lat = NAV_LAT0;
-  tracking_ecef.y = GPS_LOCAL_ECEF_ORIGIN_Y;
+  llh_nav0.lon = NAV_LON0;
-  tracking_ecef.z = GPS_LOCAL_ECEF_ORIGIN_Z;
+  /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
+  llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
 
-  ltp_def_from_ecef_i(&tracking_ltp, &tracking_ecef);
+  struct EcefCoor_i ecef_nav0;
-#endif
+  ecef_of_lla_i(&ecef_nav0, &llh_nav0);
+  ltp_def_from_ecef_i(&ltp_def, &ecef_nav0);
 }
 
-#ifdef GPS_USE_DATALINK_SMALL
 // Parse the REMOTE_GPS_SMALL datalink packet
-void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_xy)
+void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_xyz, int16_t heading)
 {
-
   // Position in ENU coordinates
-  enu_pos.x = (int32_t)((pos_xyz >> 22) & 0x3FF); // bits 31-22 x position in cm
+  enu_pos.x = (int32_t)((pos_xyz >> 21) & 0x7FF); // bits 31-21 x position in cm
-  if (enu_pos.x & 0x200) {
+  if (enu_pos.x & 0x400) {
-    enu_pos.x |= 0xFFFFFC00;  // fix for twos complements
+    enu_pos.x |= 0xFFFFF800;  // sign extend for twos complements
   }
-  enu_pos.y = (int32_t)((pos_xyz >> 12) & 0x3FF); // bits 21-12 y position in cm
+  enu_pos.y = (int32_t)((pos_xyz >> 10) & 0x7FF); // bits 20-10 y position in cm
-  if (enu_pos.y & 0x200) {
+  if (enu_pos.y & 0x400) {
-    enu_pos.y |= 0xFFFFFC00;  // fix for twos complements
+    enu_pos.y |= 0xFFFFF800;  // sign extend for twos complements
   }
-  enu_pos.z = (int32_t)(pos_xyz >> 2 & 0x3FF); // bits 11-2 z position in cm
+  enu_pos.z = (int32_t)(pos_xyz & 0x3FF); // bits 9-0 z position in cm
-  // bits 1 and 0 are free
-
-  // printf("ENU Pos: %u (%d, %d, %d)\n", pos_xyz, enu_pos.x, enu_pos.y, enu_pos.z);
 
   // Convert the ENU coordinates to ECEF
-  ecef_of_enu_point_i(&ecef_pos, &tracking_ltp, &enu_pos);
+  ecef_of_enu_point_i(&gps.ecef_pos, &ltp_def, &enu_pos);
-  gps.ecef_pos = ecef_pos;
+  SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT);
 
-  lla_of_ecef_i(&lla_pos, &ecef_pos);
+  lla_of_ecef_i(&gps.lla_pos, &gps.ecef_pos);
-  gps.lla_pos = lla_pos;
   SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT);
 
-  enu_speed.x = (int32_t)((speed_xy >> 22) & 0x3FF); // bits 31-22 speed x in cm/s
+  enu_speed.x = (int32_t)((speed_xyz >> 21) & 0x7FF); // bits 31-21 speed x in cm/s
-  if (enu_speed.x & 0x200) {
+  if (enu_speed.x & 0x400) {
-    enu_speed.x |= 0xFFFFFC00;  // fix for twos complements
+    enu_speed.x |= 0xFFFFF800;  // sign extend for twos complements
   }
-  enu_speed.y = (int32_t)((speed_xy >> 12) & 0x3FF); // bits 21-12 speed y in cm/s
+  enu_speed.y = (int32_t)((speed_xyz >> 10) & 0x7FF); // bits 20-10 speed y in cm/s
-  if (enu_speed.y & 0x200) {
+  if (enu_speed.y & 0x400) {
-    enu_speed.y |= 0xFFFFFC00;  // fix for twos complements
+    enu_speed.y |= 0xFFFFF800;  // sign extend for twos complements
+  }
+  enu_speed.z = (int32_t)((speed_xyz) & 0x3FF); // bits 9-0 speed z in cm/s
+  if (enu_speed.z & 0x200) {
+    enu_speed.z |= 0xFFFFFC00;  // sign extend for twos complements
   }
-  enu_speed.z = 0;
 
-  // printf("ENU Speed: %u (%d, %d, %d)\n", speed_xy, enu_speed.x, enu_speed.y, enu_speed.z);
+  ecef_of_enu_vect_i(&gps.ecef_vel , &ltp_def , &enu_speed);
-
-  ecef_of_enu_vect_i(&gps.ecef_vel , &tracking_ltp , &enu_speed);
   SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT);
 
-  gps.hmsl = tracking_ltp.hmsl + enu_pos.z * 10; // TODO: try to compensate for the loss in accuracy
+  // todo check correct
+  gps.ned_vel.x = enu_speed.x;
+  gps.ned_vel.y = enu_speed.y;
+  gps.ned_vel.z = -enu_speed.z;
+  SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT);
+
+  gps.hmsl = ltp_def.hmsl + enu_pos.z * 10;
   SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT);
 
-  gps.course = (int32_t)((speed_xy >> 2) & 0x3FF); // bits 11-2 heading in rad*1e2
+  gps.course = ((int32_t)heading) * 1e3;
-  if (gps.course & 0x200) {
-    gps.course |= 0xFFFFFC00;  // fix for twos complements
-  }
-  // printf("Heading: %d\n", gps.course);
-  gps.course *= 1e5;
   SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT);
 
   gps.num_sv = num_sv;
-  gps.tow = 0; // set time-of-weak to 0
+  gps.tow = gps_tow_from_sys_ticks(sys_time.nb_tick); // todo check
   gps.fix = GPS_FIX_3D; // set 3D fix to true
   gps_available = TRUE; // set GPS available to true
 
@@ -143,7 +125,6 @@ void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_x
   }
   AbiSendMsgGPS(GPS_DATALINK_ID, now_ts, &gps);
 }
-#endif
 
 /** Parse the REMOTE_GPS datalink packet */
 void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t ecef_z, int32_t lat, int32_t lon,
@@ -168,10 +149,20 @@ void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t e
   gps.ecef_vel.z = ecef_zd;
   SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT);
 
+  struct LtpDef_i ref_ltp;
+  ltp_def_from_ecef_i(&ref_ltp, &gps.ecef_pos);
+  ned_of_ecef_vect_i(&gps.ned_vel, &ref_ltp, &gps.ecef_vel);
+  SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT);
+
   gps.course = course;
   SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT);
+
   gps.num_sv = numsv;
-  gps.tow = tow;
+  if (tow == 0) {
+    gps.tow = gps_tow_from_sys_ticks(sys_time.nb_tick); //tow;
+  } else {
+    gps.tow = gps_tow_from_sys_ticks(sys_time.nb_tick); //tow;
+  }
   gps.fix = GPS_FIX_3D;
   gps_available = TRUE;
 

sw/airborne/subsystems/gps/gps_datalink.h

@@ -35,9 +35,9 @@
 #define GPS_NB_CHANNELS 0
 
 extern bool_t gps_available;
-#ifdef GPS_USE_DATALINK_SMALL
+
-void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_xy);
+extern void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_xyz, int16_t heading);
-#endif
+
 extern void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t ecef_z,
                                int32_t lat, int32_t lon, int32_t alt, int32_t hmsl,
                                int32_t ecef_xd, int32_t ecef_yd, int32_t ecef_zd,

sw/airborne/subsystems/ins/ins_int.c

@@ -98,6 +98,10 @@ PRINT_CONFIG_MSG("INS_SONAR_UPDATE_ON_AGL defaulting to FALSE")
 #define INS_VFF_R_GPS 2.0
 #endif
 
+#ifndef INS_VFF_VZ_R_GPS
+#define INS_VFF_VZ_R_GPS 2.0
+#endif
+
 /** maximum number of propagation steps without any updates in between */
 #ifndef INS_MAX_PROPAGATION_STEPS
 #define INS_MAX_PROPAGATION_STEPS 200
@@ -374,6 +378,7 @@ void ins_int_update_gps(struct GpsState *gps_s)
 
 #if INS_USE_GPS_ALT
   vff_update_z_conf(((float)gps_pos_cm_ned.z) / 100.0, INS_VFF_R_GPS);
+  vff_update_vz_conf(((float)gps_speed_cm_s_ned.z) / 100.0, INS_VFF_VZ_R_GPS);
 #endif
 
 #if USE_HFF