Merge pull request #151 from aabadie/sailbot_data

protocol: sailbot: use SAILBOT_DATA message type to send heading with GPS coordinates
DotBots · Feb 17, 2023 · 2f666ae · 2f666ae
2 parents 3902d57 + f0c1a30
commit 2f666ae
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 18 deletions.
diff --git a/drv/protocol.h b/drv/protocol.h
@@ -16,23 +16,24 @@
 
 //=========================== defines ==========================================
 
-#define DB_FIRMWARE_VERSION  (6)                   ///< Version of the firmware
+#define DB_FIRMWARE_VERSION  (7)                   ///< Version of the firmware
 #define DB_SWARM_ID          (0x0000)              ///< Default swarm ID
 #define DB_BROADCAST_ADDRESS 0xffffffffffffffffUL  ///< Broadcast address
 #define DB_GATEWAY_ADDRESS   0x0000000000000000UL  ///< Gateway address
 #define DB_MAX_WAYPOINTS     (8)                   ///< Max number of waypoints
 
 typedef enum {
-    DB_PROTOCOL_CMD_MOVE_RAW  = 0,  ///< Move raw command type
-    DB_PROTOCOL_CMD_RGB_LED   = 1,  ///< RGB LED command type
-    DB_PROTOCOL_LH2_RAW_DATA  = 2,  ///< Lighthouse 2 raw data
-    DB_PROTOCOL_LH2_LOCATION  = 3,  ///< Lighthouse processed locations
-    DB_PROTOCOL_ADVERTISEMENT = 4,  ///< DotBot advertisements
-    DB_PROTOCOL_GPS_LOCATION  = 5,  ///< GPS data from SailBot
-    DB_PROTOCOL_DOTBOT_DATA   = 6,  ///< DotBot specific data (for now location and direction)
-    DB_PROTOCOL_CONTROL_MODE  = 7,  ///< Robot remote control mode (automatic or manual)
-    DB_PROTOCOL_LH2_WAYPOINTS = 8,  ///< List of LH2 waypoints to follow
-    DB_PROTOCOL_GPS_WAYPOINTS = 9,  ///< List of GPS waypoints to follow
+    DB_PROTOCOL_CMD_MOVE_RAW  = 0,   ///< Move raw command type
+    DB_PROTOCOL_CMD_RGB_LED   = 1,   ///< RGB LED command type
+    DB_PROTOCOL_LH2_RAW_DATA  = 2,   ///< Lighthouse 2 raw data
+    DB_PROTOCOL_LH2_LOCATION  = 3,   ///< Lighthouse processed locations
+    DB_PROTOCOL_ADVERTISEMENT = 4,   ///< DotBot advertisements
+    DB_PROTOCOL_GPS_LOCATION  = 5,   ///< GPS data from SailBot
+    DB_PROTOCOL_DOTBOT_DATA   = 6,   ///< DotBot specific data (for now location and direction)
+    DB_PROTOCOL_CONTROL_MODE  = 7,   ///< Robot remote control mode (automatic or manual)
+    DB_PROTOCOL_LH2_WAYPOINTS = 8,   ///< List of LH2 waypoints to follow
+    DB_PROTOCOL_GPS_WAYPOINTS = 9,   ///< List of GPS waypoints to follow
+    DB_PROTOCOL_SAILBOT_DATA  = 10,  ///< SailBot specific data (for now GPS and direction)
 } command_type_t;
 
 typedef enum {

diff --git a/projects/03app_sailbot/03app_sailbot.c b/projects/03app_sailbot/03app_sailbot.c
@@ -91,7 +91,7 @@ static float  calculate_error(float heading, float bearing);
 static int8_t map_error_to_rudder_angle(float error);
 static void   _timeout_check(void);
 static void   _advertise(void);
-static void   _send_gps_data(const nmea_gprmc_t *data);
+static void   _send_gps_data(const nmea_gprmc_t *data, uint16_t heading);
 
 //=========================== main =========================================
 
@@ -226,7 +226,7 @@ void control_loop_callback(void) {
     // get heading
     float heading = lis2mdl_last_heading();
 
-    _send_gps_data(gps_data);
+    _send_gps_data(gps_data, (uint16_t)(heading * 180 / M_PI));
 
     if (!_sailbot_vars.autonomous_operation) {
         // Do nothing if not in autonomous operation
@@ -282,16 +282,17 @@ void control_loop_callback(void) {
     }
 }
 
-static void _send_gps_data(const nmea_gprmc_t *data) {
+static void _send_gps_data(const nmea_gprmc_t *data, uint16_t heading) {
     int32_t latitude  = (int32_t)(data->latitude * 1e6);
     int32_t longitude = (int32_t)(data->longitude * 1e6);
 
-    db_protocol_header_to_buffer(_sailbot_vars.radio_buffer, DB_BROADCAST_ADDRESS, SailBot, DB_PROTOCOL_GPS_LOCATION);
+    db_protocol_header_to_buffer(_sailbot_vars.radio_buffer, DB_BROADCAST_ADDRESS, SailBot, DB_PROTOCOL_SAILBOT_DATA);
 
-    memcpy(_sailbot_vars.radio_buffer + sizeof(protocol_header_t), &latitude, sizeof(int32_t));
-    memcpy(_sailbot_vars.radio_buffer + sizeof(protocol_header_t) + sizeof(int32_t), &longitude, sizeof(int32_t));
+    memcpy(_sailbot_vars.radio_buffer + sizeof(protocol_header_t), &heading, sizeof(uint16_t));
+    memcpy(_sailbot_vars.radio_buffer + sizeof(protocol_header_t) + sizeof(uint16_t), &latitude, sizeof(int32_t));
+    memcpy(_sailbot_vars.radio_buffer + sizeof(protocol_header_t) + sizeof(uint16_t) + sizeof(int32_t), &longitude, sizeof(int32_t));
 
-    size_t length = sizeof(protocol_header_t) + 2 * sizeof(int32_t);
+    size_t length = sizeof(protocol_header_t) + sizeof(uint16_t) + 2 * sizeof(int32_t);
     db_radio_rx_disable();
     db_radio_tx(_sailbot_vars.radio_buffer, length);
     db_radio_rx_enable();