diff --git a/ground/platformio.ini b/ground/platformio.ini index ab79ec49..36a2450c 100644 --- a/ground/platformio.ini +++ b/ground/platformio.ini @@ -19,6 +19,18 @@ build_unflags = -std=gnu++11 build_src_filter = + +[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 = + + [env:feather] platform = atmelsam board = adafruit_feather_m0 @@ -43,4 +55,4 @@ board = adafruit_feather_m0 framework = arduino build_src_filter = + build_flags = -DIS_TEST -; test_ignore = test_local \ No newline at end of file +; test_ignore = test_local diff --git a/ground/src/feather_duo/Output.cpp b/ground/src/feather_duo/Output.cpp index 9274dfed..b521df26 100644 --- a/ground/src/feather_duo/Output.cpp +++ b/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(-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); +} diff --git a/ground/src/feather_duo/Output.h b/ground/src/feather_duo/Output.h index 24a43195..ef1cce90 100644 --- a/ground/src/feather_duo/Output.h +++ b/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); \ No newline at end of file +void printPacketJson(FullTelemetryData const& packet); +void printDronePacketJson(FullTelemetryData const& packet); diff --git a/ground/src/feather_duo/Packet.cpp b/ground/src/feather_duo/Packet.cpp index 4bcd452f..c73619a1 100644 --- a/ground/src/feather_duo/Packet.cpp +++ b/ground/src/feather_duo/Packet.cpp @@ -82,4 +82,26 @@ FullTelemetryData DecodePacket(const TelemetryPacket& packet, float frequency) { data.freq = frequency; return data; -} \ No newline at end of file +} + +FullTelemetryData DecodeDronePacket(const DroneTelemetryPacket& packet, float frequency) { + int64_t start_printing = millis(); + FullTelemetryData data; + // GPS + data.altitude = static_cast(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(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; +} diff --git a/ground/src/feather_duo/Packet.h b/ground/src/feather_duo/Packet.h index f951aebc..31d4ae13 100644 --- a/ground/src/feather_duo/Packet.h +++ b/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); diff --git a/ground/src/feather_duo/main.cpp b/ground/src/feather_duo/main.cpp index e59ecd0f..d4c6efd6 100644 --- a/ground/src/feather_duo/main.cpp +++ b/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 booster_cmds; Queue 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,9 +245,15 @@ 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); } @@ -229,4 +261,4 @@ void setup() { void loop() { delay(10000); -} \ No newline at end of file +}