Update feature duo to receive drone packets

ground/platformio.ini

@@ -19,6 +19,18 @@ build_unflags =
     -std=gnu++11
 build_src_filter =  +<feather_duo/>
 
+[env:feather_duo_drone]
+platform = espressif32
+board = esp32-s3-devkitc-1
+framework = arduino
+build_flags =
+    -DARDUINO_USB_CDC_ON_BOOT=1
+    -std=gnu++2a
+    -DIS_DRONE
+build_unflags =
+    -std=gnu++11
+build_src_filter =  +<feather_duo/>
+
 [env:feather]
 platform = atmelsam
 board = adafruit_feather_m0
@@ -43,4 +55,4 @@ board = adafruit_feather_m0
 framework = arduino
 build_src_filter =  +<feather/>
 build_flags = -DIS_TEST
-; test_ignore = test_local
+; test_ignore = test_local

ground/src/feather_duo/Output.cpp

@@ -108,3 +108,33 @@ void printPacketJson(FullTelemetryData const& packet) {
     // printJSONField("pyro_d", packet.pyros[3], false);
     // Serial.println("}}");
 }
+
+void printDronePacketJson(FullTelemetryData const& packet) {
+    bool is_heartbeat = packet.FSM_State == static_cast<uint8_t>(-1);
+    char buff[1024]{}; // Leaving the same buffer size as PacketJson
+
+    int len = sprintf(buff, R"({"type": "data", "value": 
+        {"barometer_altitude": %f, "latitude": %f, 
+        "longitude": %f, "altitude": %i, 
+        "FSM_State": %i, "tilt_angle": %f, 
+        "frequency": %f, "RSSI": %f, 
+        "sat_count": %i, "is_sustainer": %i,
+        }})",
+
+    // Barometer
+    packet.barometer_altitude,
+
+    // GPS
+    packet.latitude,
+    packet.longitude,
+    packet.altitude,
+    // Other data
+    packet.FSM_State,
+    packet.tilt_angle,
+    packet.freq,
+    packet.rssi,
+    packet.sat_count,
+    packet.is_sustainer
+    );
+    Serial.println(buff);
+}

ground/src/feather_duo/Output.h

@@ -15,4 +15,5 @@ static constexpr const char* json_set_frequency_failure = R"({"type": "freq_erro
 static constexpr const char* json_receive_failure = R"({"type": "receive_error", "error": "recv failed"})";
 static constexpr const char* json_send_failure = R"({"type": "send_error", "error": "command_retries_exceded"})";
 
-void printPacketJson(FullTelemetryData const& packet);
+void printPacketJson(FullTelemetryData const& packet);
+void printDronePacketJson(FullTelemetryData const& packet);

ground/src/feather_duo/Packet.cpp

@@ -82,4 +82,26 @@ FullTelemetryData DecodePacket(const TelemetryPacket& packet, float frequency) {
     data.freq = frequency;
 
     return data;
-}
+}
+
+FullTelemetryData DecodeDronePacket(const DroneTelemetryPacket& packet, float frequency) {
+    int64_t start_printing = millis();
+    FullTelemetryData data;
+    // GPS
+    data.altitude = static_cast<float>(packet.alt & 0xfffe); // Only convert top 15 bits
+    data.latitude = ConvertGPS(packet.lat);
+    data.longitude = ConvertGPS(packet.lon);
+    data.cmd_ack = packet.alt & 1; // Ack bit
+    // Barometer
+    data.barometer_altitude = convert_range<int16_t>(packet.baro_alt, 1 << 17);
+    // Tilt & FSM
+    data.tilt_angle = ((float)((packet.tilt_fsm >> 4) & 0x0fff) / 0x0fff) * 180;
+    data.FSM_State = packet.tilt_fsm & 0x000f;
+
+    // Other data
+    data.gps_fixtype = packet.callsign_gpsfix_satcount >> 1 & 0b0111;
+    data.is_sustainer = (packet.callsign_gpsfix_satcount & 0b1);
+    data.sat_count = (packet.callsign_gpsfix_satcount >> 4) & 0b1111;
+    data.freq = frequency;
+    return data;
+}

ground/src/feather_duo/Packet.h

@@ -48,6 +48,16 @@ struct TelemetryPacket {
 };
 
 
+struct DroneTelemetryPacket {
+    int32_t lon; 
+    int32_t lat;
+    uint16_t alt; //15 bit meters, 1 bit command ack
+    uint16_t baro_alt;
+    uint16_t tilt_fsm; //12 bits tilt | 4 bits FSM
+    uint8_t callsign_gpsfix_satcount; //3 bits gpsfix, 4 bits sat count, 1 bit is_sustainer_callsign
+};
+
+
 struct FullTelemetryData {
 
     systime_t timestamp;  //[0, 2^32]
@@ -87,3 +97,4 @@ struct FullTelemetryData {
 };
 
 FullTelemetryData DecodePacket(const TelemetryPacket& packet, float frequency);
+FullTelemetryData DecodeDronePacket(const DroneTelemetryPacket& packet, float frequency);

ground/src/feather_duo/main.cpp

@@ -8,8 +8,10 @@
 #include"Command.h"
 #include"Queue.h"
 
-constexpr uint32_t BOOSTER_FREQ = 425150000;
-constexpr uint32_t SUSTAINER_FREQ = 421150000;
+constexpr uint32_t ROCKET_BOOSTER_FREQ = 425150000;
+constexpr uint32_t ROCKET_SUSTAINER_FREQ = 421150000;
+constexpr uint32_t DRONE_BOOSTER_FREQ = 0;
+constexpr uint32_t DRONE_SUSTAINER_FREQ = 0;
 Queue<TelemetryCommand> booster_cmds;
 Queue<TelemetryCommand> sustainer_cmds;
 
@@ -38,7 +40,24 @@ bool init_radio(SX1268& radio, uint32_t frequency) {
     return true;
 }
 
-void Radio_Rx_Thread(void * arg) {
+void Drone_Rx_Thread(void * arg) {
+    RadioConfig* cfg = (RadioConfig*)arg;
+    bool led_state = false;
+
+    while(true) {
+        DroneTelemetryPacket packet{};
+        SX1268Error res = cfg->radio->recv((uint8_t*)&packet, sizeof(packet), -1);
+        if(res == SX1268Error::NoError) {
+            led_state = !led_state;
+            digitalWrite(cfg->indicator_led, led_state);
+            FullTelemetryData data = DecodeDronePacket(packet, cfg->frequency / 1e6);
+            data.rssi = cfg->radio->get_last_snr();
+            printDronePacketJson(data);
+        }
+    }
+}
+
+void Rocket_Rx_Thread(void * arg) {
     RadioConfig* cfg = (RadioConfig*)arg;
     bool led_state = false;
     bool reset_state = false;
@@ -182,6 +201,13 @@ void Management_Thread(void * arg) {
 
 void setup() {
     Serial.begin(460800);
+
+    uint32_t BOOSTER_FREQ = ROCKET_BOOSTER_FREQ;
+    uint32_t SUSTAINER_FREQ = ROCKET_BOOSTER_FREQ;
+    #ifdef IS_DRONE
+        BOOSTER_FREQ = DRONE_BOOSTER_FREQ;
+        SUSTAINER_FREQ = DRONE_SUSTAINER_FREQ;
+    #endif
 
     SPIClass SPI0(HSPI);
     SPIClass SPI1(FSPI);
@@ -219,14 +245,20 @@ void setup() {
         .indicator_led=Pins::LED_BLUE,
     };
 
-    xTaskCreatePinnedToCore(Radio_Rx_Thread, "Radio0_thread", 8192, &booster_cfg, 0, nullptr, 1);
-    xTaskCreatePinnedToCore(Radio_Rx_Thread, "Radio1_thread", 8192, &sustainer_cfg, 0, nullptr, 1);
-    xTaskCreatePinnedToCore(Management_Thread, "Management_thread", 8192, nullptr, 0, nullptr, 1);
+    #ifdef IS_DRONE
+        xTaskCreatePinnedToCore(Drone_Rx_Thread, "Drone0_thread", 8192, nullptr, 0, nullptr, 1);
+        xTaskCreatePinnedToCore(Drone_Rx_Thread, "Drone1_thread", 8192, nullptr, 0, nullptr, 1);    
+    #else  
+        xTaskCreatePinnedToCore(Rocket_Rx_Thread, "Rocket0_thread", 8192, &booster_cfg, 0, nullptr, 1);
+        xTaskCreatePinnedToCore(Rocket_Rx_Thread, "Rocket1_thread", 8192, &sustainer_cfg, 0, nullptr, 1);
+        xTaskCreatePinnedToCore(Management_Thread, "Management_thread", 8192, nullptr, 0, nullptr, 1);
+    #endif
+
     while(true) {
         delay(10000);
     }
 }
 
 void loop() {
     delay(10000);
-}
+}