[messages][gps] add GPS_INJECT message and gps_inject_data

paparazzi · Sep 13, 2015 · c9518bf · c9518bf
1 parent 3d4028d
commit c9518bf
Show file tree

Hide file tree

Showing 4 changed files with 28 additions and 0 deletions.
diff --git a/conf/messages.xml b/conf/messages.xml
@@ -2534,6 +2534,12 @@
    <field name="pan" type="int8"/>
  </message>
 
+ <message name="GPS_INJECT" id="153" link="forwarded">
+   <field name="ac_id" type="uint8"/>
+   <field name="packet_id" type="uint8"/>
+   <field name="data" type="uint8[]"/>
+ </message>
+
 </msg_class>
 
 

diff --git a/sw/airborne/firmwares/rotorcraft/datalink.c b/sw/airborne/firmwares/rotorcraft/datalink.c
@@ -164,6 +164,19 @@ void dl_parse_msg(void)
         DL_REMOTE_GPS_tow(dl_buffer),
         DL_REMOTE_GPS_course(dl_buffer));
       break;
+#endif
+#if USE_GPS
+    case DL_GPS_INJECT :
+      // Check if the GPS is for this AC
+      if (DL_GPS_INJECT_ac_id(dl_buffer) != AC_ID) { break; }
+
+      // GPS parse data
+      gps_inject_data(
+        DL_GPS_INJECT_packet_id(dl_buffer),
+        DL_GPS_INJECT_data_length(dl_buffer),
+        DL_GPS_INJECT_data(dl_buffer)
+        );
+      break;
 #endif
     default:
       break;

diff --git a/sw/airborne/subsystems/gps.c b/sw/airborne/subsystems/gps.c
@@ -191,3 +191,10 @@ uint32_t gps_tow_from_sys_ticks(uint32_t sys_ticks)
 
   return itow_now;
 }
+
+/**
+ * Default parser for GPS injected data
+ */
+void WEAK gps_inject_data(uint8_t packet_id __attribute__((unused)), uint8_t length __attribute__((unused)), uint8_t *data __attribute__((unused))){
+
+}
diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h
@@ -108,6 +108,8 @@ extern void gps_init(void);
 /** GPS model specific init implementation */
 extern void gps_impl_init(void);
 
+/** GPS packet injection (default empty) */
+extern void gps_inject_data(uint8_t packet_id, uint8_t length, uint8_t *data);
 
 /** GPS timeout in seconds */
 #ifndef GPS_TIMEOUT