Cansat
/
ESP32-CAN
2
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

drone comms update. add change bitrate custom function to radio. change drone comms. implement change datarate

This commit is contained in:
franchioping 2026-04-02 18:57:52 +01:00
parent 9184b142f2
commit aa2a3710d0
7 changed files with 126 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
components/RFM69_LowPowerLab

@ -1 +1 @@
Subproject commit 240306de6ebe5ef16ab474f8862366f96b54a711
Subproject commit 034caf2f11a15c67a8a1985b9a02ac3d5026671b

2
components/drone_comms

@ -1 +1 @@
Subproject commit 1a1fccca83a4a24670715efa4b532d8a7c4e51ca
Subproject commit d7debd6a59298850dd7371a90ed331175b64ab12

2
components/drone_controller

@ -1 +1 @@
Subproject commit 17f62e4827fb94694799f091ab93c09628effef8
Subproject commit 9d8fccc2e3981746a077b47b57a88701d9f3f502

4
main/env_sens.cpp
View File

@ -29,7 +29,7 @@ void setup() {
ESP_LOGI(TAG, "BARO SETUP COMPLETE.");
bme.setSampling(Adafruit_BME280::MODE_NORMAL, Adafruit_BME280::SAMPLING_X8,
Adafruit_BME280::SAMPLING_X8, Adafruit_BME280::SAMPLING_NONE,
Adafruit_BME280::FILTER_OFF, Adafruit_BME280::STANDBY_MS_10);
Adafruit_BME280::FILTER_OFF, Adafruit_BME280::STANDBY_MS_20);
bme_temp->printSensorDetails();
bme_pressure->printSensorDetails();
@ -109,7 +109,7 @@ void baro_poll_task(void *_) {
}
// BME280 in your config has a 10ms standby, so 20ms-50ms poll is ideal
vTaskDelay(pdMS_TO_TICKS(20));
vTaskDelay(pdMS_TO_TICKS(50));
}
}

10
main/imu.h
View File

@ -1,10 +1,16 @@
#pragma once
#include <cmath>
#include "drone_controller.h"
#include <cstdint>
#ifdef LOW
#undef LOW
#endif
#ifdef HIGH
#undef HIGH
#endif
#include "BNO08x.hpp"
#include "drone_controller.h"
struct imu_state {
Vec3C accel = {0, 0, 0};

79
main/main.cpp
View File

@ -1,21 +1,27 @@
#include "driver/gpio.h"
#include "drone_comms.h"
#include "esp32-hal.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/idf_additions.h"
#include "freertos/projdefs.h"
#include "freertos/task.h"
#include "imu.h"
#include <Arduino.h>
#include <cstddef>
#include <cstdint>
#include "env_sens.h"
#include "gps.h"
#include "nav.h"
#include "radio.h"
#include "imu.h"
static const char *TAG = "MAIN";
void handle_packet(uint8_t *packet_addr);
extern "C" void app_main(void) {
nav_mutex = xSemaphoreCreateMutex();
@ -26,18 +32,18 @@ extern "C" void app_main(void) {
static imu_state imu_state;
auto _ = setup_imu(&imu_state);
// xTaskCreatePinnedToCore(radio_rx_task, // Function name
// "radio_rx", // Name for debugging
// 4096, // Stack size in bytes
// NULL, // Parameters
// 1, // Priority (higher = more urgent)
// NULL, // Task handle
// 1 // Core ID
// );
//
xTaskCreatePinnedToCore(radio_task, // Function name
"radio_rxtx", // Name for debugging
4096, // Stack size in bytes
NULL, // Parameters
5, // Priority (higher = more urgent)
NULL, // Task handle
1 // Core ID
);
xTaskCreate(env_sens::baro_poll_task, "baro_poll", 8192, NULL, 1, NULL);
xTaskCreate(gps_poll_task, "gps_poll", 8192, NULL, 1, NULL);
xTaskCreate(gps_poll_task, "gps_poll", 8192, NULL, 5, NULL);
ESP_LOGI("MAIN", "All tasks spawned. Main loop free.");
@ -45,15 +51,9 @@ extern "C" void app_main(void) {
Eigen::Vector3f local_vel = {0, 0, 0};
bool nav_data_ready = false;
while (true) {
// if (controller_input_semaphore &&
// xSemaphoreTake(controller_input_semaphore, (TickType_t)30) == pdTRUE)
// {
// inp = current_controller_input;
// xSemaphoreGive(controller_input_semaphore);
//
// ESP_LOGI(TAG, "CONT INPUT: (%f, %f), (%f,%f)", inp.lx, inp.ly, inp.rx,
// inp.ry);
// }
if (xQueueReceive(packet_tx_queue, &packet_data[0], 1)) {
handle_packet(&packet_data[0]);
}
if (gps_mutex && xSemaphoreTake(gps_mutex, (TickType_t)10) == pdTRUE) {
if (gps->gps_avaliable()) {
@ -90,3 +90,42 @@ extern "C" void app_main(void) {
vTaskDelay(pdMS_TO_TICKS(1000));
}
}
void handle_packet(uint8_t *packet_addr) {
PACKET_TYPE packet_type = *((PACKET_TYPE *)packet_addr);
if (packet_type == PACKET_TYPE::COMMAND_REQUEST) {
packet_command_request *packet =
(packet_command_request *)(packet_addr + sizeof(PACKET_TYPE));
PACKET_TYPE requested_type = packet->packet_requested;
std::pair<uint8_t *, size_t> resp_packet = {nullptr, 0};
if (requested_type == PACKET_TYPE::INFO_DRONE_POSITION) {
resp_packet = create_packet_pooled(
PACKET_TYPE::INFO_DRONE_POSITION,
packet_info_drone_position{
{nav_filter.position.x(), nav_filter.position.y(),
nav_filter.position.z()},
{nav_filter.velocity.x(), nav_filter.velocity.y(),
nav_filter.velocity.z()},
{0.0, 0.0, 0.0}});
}
if (requested_type == PACKET_TYPE::INFO_DRONE_STATUS) {
if (gps_mutex && xSemaphoreTake(gps_mutex, (TickType_t)20) == pdTRUE) {
resp_packet = create_packet_pooled(
PACKET_TYPE::INFO_DRONE_STATUS,
packet_info_drone_status{
{gps->origin_long, gps->origin_lat}, millis(), 0});
xSemaphoreGive(gps_mutex);
}
}
if (resp_packet.first != nullptr) {
xQueueSend(packet_tx_queue, resp_packet.first, portMAX_DELAY);
}
}
}

66
main/radio.cpp
View File

@ -27,6 +27,12 @@ static const char *TAG = "RADIO_TASK";
#define PACKET_QUEUE_CAP 10
static bool is_pending_switch = false;
static uint32_t target_bitrate = 0;
static uint32_t switch_at_ms = 0;
static uint32_t rollback_at_ms = 0;
static bool confirmed_at_new_rate = false;
void radio_task(void *pvParameters) {
ESP_LOGI(TAG, "Radio Task Started on Core %d", xPortGetCoreID());
packet_rx_queue = xQueueCreate(PACKET_QUEUE_CAP, MAX_PACKET_SIZE);
@ -49,6 +55,7 @@ void radio_task(void *pvParameters) {
if (radio.initialize(FREQUENCY, NODEID, NETWORKID)) {
radio.setHighPower(true);
radio.setCustomBitrate(DEFAULT_COMMS_BITRATE);
ESP_LOGI(TAG, "Radio Initialized. Version: 0x%02X", radio.readReg(0x10));
} else {
ESP_LOGE(TAG, "Radio Init FAILED! Restarting.");
@ -56,26 +63,36 @@ void radio_task(void *pvParameters) {
}
while (1) {
uint32_t now = millis();
if (radio.receiveDone()) {
// ESP_LOGD(TAG, "Packet [ID:%d] RSSI:%d LEN:%d", radio.SENDERID,
// radio.RSSI,
// radio.DATALEN);
// If we receive ANY valid packet while in probation, confirm the switch
// (Bit-rate switching)
if (is_pending_switch && now > switch_at_ms) {
confirmed_at_new_rate = true;
ESP_LOGI(TAG, "New bitrate confirmed by valid packet.");
}
ESP_LOGD(TAG, "Packet [ID:%d] RSSI:%d LEN:%d", radio.SENDERID, radio.RSSI,
radio.DATALEN);
memset(packet_data, '\0', sizeof(packet_data));
memcpy(packet_data, radio.DATA, radio.DATALEN);
PACKET_TYPE packet_type = *((PACKET_TYPE *)&packet_data[0]);
if (packet_type == CONTROLLER_INPUT) {
if (packet_type == COMMAND_CHANGE_DATARATE) {
packet_command_datarate *cmd =
(packet_command_datarate *)(&packet_data[0] + sizeof(PACKET_TYPE));
target_bitrate = cmd->target_bitrate;
packet_controller_input *inp =
(packet_controller_input *)(&packet_data[0] + sizeof(PACKET_TYPE));
if (xSemaphoreTake(controller_input_semaphore, (TickType_t)50) ==
pdTRUE) {
current_controller_input = *inp;
xSemaphoreGive(controller_input_semaphore);
}
switch_at_ms = now + cmd->ms_delay;
rollback_at_ms = now + cmd->ms_rollback;
is_pending_switch = true;
confirmed_at_new_rate = false;
ESP_LOGI(TAG, "Datarate change requested: %d. Switching in 100ms...",
target_bitrate);
} else {
xQueueSend(packet_rx_queue, &packet_data[0], portMAX_DELAY);
}
@ -84,11 +101,36 @@ void radio_task(void *pvParameters) {
radio.sendACK();
}
}
// Send packets that were queued up for sending
if (xQueueReceive(packet_tx_queue, &packet_data[0], 1)) {
PACKET_TYPE packet_type = *((PACKET_TYPE *)&packet_data[0]);
radio.send(GROUNDID, &packet_data[0], get_packet_size(packet_type));
}
// --- STATE MACHINE FOR BITRATE SWITCHING ---
// 1. Execute the Switch
if (is_pending_switch && now >= switch_at_ms && !confirmed_at_new_rate) {
// We only want to trigger the register write once
radio.setCustomBitrate(target_bitrate);
switch_at_ms = 0xFFFFFFFF; // Prevent re-triggering
}
// 2. The Rollback (The Fail-safe)
if (is_pending_switch && !confirmed_at_new_rate && now > rollback_at_ms) {
ESP_LOGE(TAG,
"ROLLBACK: No confirmation at new rate. Reverting to default.");
radio.setCustomBitrate(DEFAULT_COMMS_BITRATE);
is_pending_switch = false;
}
// 3. Clear pending flag once confirmed
if (confirmed_at_new_rate) {
is_pending_switch = false;
confirmed_at_new_rate = false;
}
vTaskDelay(pdMS_TO_TICKS(10));
}
}