Merge pull request #117 from malishav/116-sailbot-send-gps-data-over-…

…radio SailBot: Send GPS data over radio
DotBots · Dec 13, 2022 · 8bab973 · 8bab973
2 parents 8ac08fa + 7e726b2
commit 8bab973
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 0 deletions.
diff --git a/drv/protocol.h b/drv/protocol.h
@@ -27,6 +27,7 @@ typedef enum {
     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
 } command_type_t;
 
 typedef enum {

diff --git a/projects/03app_sailbot/03app_sailbot.c b/projects/03app_sailbot/03app_sailbot.c
@@ -109,6 +109,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);
 
 //=========================== main =========================================
 
@@ -293,6 +294,23 @@ void control_loop_callback(void) {
     if (!_sailbot_vars.radio_override) {
         servos_rudder_turn(rudder_angle);
     }
+
+    _send_gps_data(gps_data);
+}
+
+static void _send_gps_data(const nmea_gprmc_t *data) {
+    int32_t latitude  = (int32_t)(data->latitude * 1000000);
+    int32_t longitude = (int32_t)(data->longitude * 1000000);
+
+    db_protocol_header_to_buffer(_sailbot_vars.radio_buffer, DB_BROADCAST_ADDRESS, SailBot, DB_PROTOCOL_GPS_LOCATION);
+
+    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));
+
+    size_t length = sizeof(protocol_header_t) + 2 * sizeof(int32_t);
+    db_radio_rx_disable();
+    db_radio_tx(_sailbot_vars.radio_buffer, length);
+    db_radio_rx_enable();
 }
 
 static int8_t map_error_to_rudder_angle(float error) {