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

idk if working since azores

This commit is contained in:
franchioping 2026-03-30 22:09:23 +01:00
parent 4057f0c503
commit 5be19b3f11
13 changed files with 435 additions and 173 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
components/drone_comms

@ -1 +1 @@
Subproject commit e93d423065d1f100fbd9da05383ab555261aa6d4
Subproject commit 1a1fccca83a4a24670715efa4b532d8a7c4e51ca

2
main/CMakeLists.txt
View File

@ -1,7 +1,7 @@
file(GLOB_RECURSE SOURCES "*.cpp")
idf_component_register(SRCS "${SOURCES}"
INCLUDE_DIRS "" REQUIRES drone_controller esp32_Adafruit_BME280_Library esp32_Adafruit_GPS esp32_BNO08x eigen DShotRMT RFM69_LowPowerLab)
INCLUDE_DIRS "" REQUIRES drone_controller esp32_Adafruit_BME280_Library esp32_Adafruit_GPS esp32_BNO08x eigen DShotRMT RFM69_LowPowerLab drone_comms)
target_compile_options(${COMPONENT_LIB} PRIVATE
"-Wno-gnu-array-member-paren-init"

67
main/env_sens.cpp
View File

@ -1,30 +1,35 @@
#include "env_sens.h"
#include "esp_log.h"
#include <Adafruit_BME280.h>
#include <SPI.h>
#include <Wire.h>
#define SEALEVELPRESSURE_HPA (1013.25)
#include "freertos/idf_additions.h"
#include "nav.h"
#define SEALEVELPRESSURE_HPA (1030)
Adafruit_BME280 bme; // use I2C interface
Adafruit_Sensor *bme_temp = bme.getTemperatureSensor();
Adafruit_Sensor *bme_pressure = bme.getPressureSensor();
Adafruit_Sensor *bme_humidity = bme.getHumiditySensor();
static const constexpr char *TAG = "IMU";
static const constexpr char *TAG = "BARO";
namespace env_sens {
void setup() {
baro_mutex = xSemaphoreCreateMutex();
if (!bme.begin()) {
ESP_LOGE(TAG, "Couldn't find a valid sensor");
return;
}
bme.setSampling(Adafruit_BME280::MODE_NORMAL, Adafruit_BME280::SAMPLING_X2,
Adafruit_BME280::SAMPLING_X2, Adafruit_BME280::SAMPLING_NONE,
Adafruit_BME280::FILTER_OFF,
Adafruit_BME280::STANDBY_MS_62_5);
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);
bme_temp->printSensorDetails();
bme_pressure->printSensorDetails();
@ -33,20 +38,23 @@ void setup() {
float get_temperature() {
sensors_event_t temp_event;
bme_temp->getEvent(&temp_event);
return temp_event.temperature;
}
float get_pressure() {
sensors_event_t e;
bme_pressure->getEvent(&e);
return e.pressure;
}
float calculateAltitude(float pressure, float seaLevelPressure,
float tempCelsius) {
float altitude =
(((pow((seaLevelPressure / pressure), (1.0 / 5.257))) - 1.0) *
(((std::pow((seaLevelPressure / pressure), (1.0 / 5.257))) - 1.0) *
(tempCelsius + 273.15)) /
0.0065;
return altitude;
@ -62,4 +70,47 @@ void dbg_sens() {
get_pressure(), get_altitude());
}
void baro_poll_task(void *_) {
env_sens::setup();
float last_alt = env_sens::get_altitude();
uint32_t last_time = xTaskGetTickCount();
float filtered_alt = last_alt;
const float alt_lpf = 0.1f;
while (true) {
uint32_t now = xTaskGetTickCount();
float dt = (now - last_time) * portTICK_PERIOD_MS / 1000.0f;
if (dt > 0.001f) { // Prevent division by zero
float current_alt = env_sens::get_altitude();
filtered_alt = (alt_lpf * current_alt) + (1.0f - alt_lpf) * filtered_alt;
float v_z = (filtered_alt - last_alt) / dt;
if (nav_mutex && xSemaphoreTake(nav_mutex, (TickType_t)20) == pdTRUE) {
Eigen::Vector3f baro_pos = nav_filter.position;
baro_pos.z() = filtered_alt;
Eigen::Vector3f baro_vel = nav_filter.velocity;
baro_vel.z() = v_z;
// Update the filter with Baro data
nav_filter.measure_baro(dt, baro_pos, baro_vel);
xSemaphoreGive(nav_mutex);
}
last_alt = filtered_alt;
last_time = now;
}
// BME280 in your config has a 10ms standby, so 20ms-50ms poll is ideal
vTaskDelay(pdMS_TO_TICKS(20));
}
}
} // namespace env_sens

4
main/env_sens.h
View File

@ -1,5 +1,6 @@
#pragma once
#include "freertos/idf_additions.h"
namespace env_sens {
void setup();
@ -11,5 +12,8 @@ float get_pressure();
void dbg_sens();
float get_altitude();
void baro_poll_task(void *_);
} // namespace env_sens
inline SemaphoreHandle_t baro_mutex = NULL;

36
main/gps.cpp Normal file
View File

@ -0,0 +1,36 @@
#include "gps.h"
#include "esp_log.h"
#include "nav.h"
static const char *TAG = "GPS_TASK";
void gps_poll_task(void *_) {
gps = new GPS();
gps->begin();
gps_mutex = xSemaphoreCreateMutex();
ESP_LOGI(TAG, "GPS TASK INIT.");
while (true) {
if (xSemaphoreTake(gps_mutex, (TickType_t)1) == pdTRUE) {
// ESP_LOGI(TAG, "Polling gps.");
gps->poll();
if (gps->gps_avaliable() && nav_mutex &&
xSemaphoreTake(nav_mutex, (TickType_t)2) == pdTRUE) {
auto vel = gps->velocity().value_or(Eigen::Vector2f::Zero());
auto coords = gps->get_coordinates();
if (coords.has_value()) {
nav_filter.measure_gps(1.0, gps->get_coordinates().value(),
Eigen::Vector3f(vel.x(), vel.y(), 0.0));
}
xSemaphoreGive(nav_mutex);
}
xSemaphoreGive(gps_mutex);
} else {
ESP_LOGE(TAG, "FAILED TO GET GPS MUTEX");
}
vTaskDelay(pdMS_TO_TICKS(100));
}
}

51
main/gps.h
View File

@ -11,6 +11,7 @@
#include <Eigen/Dense>
#include "HardwareSerial.h"
#include "esp_log.h"
#include <Adafruit_GPS.h>
#include <cmath>
@ -22,7 +23,7 @@ const float KNOTS_TO_M_SEC = 0.5144444f;
const float earth_radius = 6371000.0f;
struct GPS {
Adafruit_GPS gps;
Adafruit_GPS *gps;
float origin_lat;
float origin_long;
@ -33,22 +34,22 @@ struct GPS {
}
void begin() {
this->gps = Adafruit_GPS(&Serial2);
this->gps.begin(9600);
this->gps.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
this->gps = new Adafruit_GPS(&Serial2);
this->gps->begin(9600);
this->gps->sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
this->gps.sendCommand(PMTK_API_SET_FIX_CTL_5HZ);
this->gps.sendCommand(PMTK_SET_NMEA_UPDATE_5HZ);
this->gps->sendCommand(PMTK_API_SET_FIX_CTL_5HZ);
this->gps->sendCommand(PMTK_SET_NMEA_UPDATE_5HZ);
}
bool gps_avaliable() { return this->gps.fix; }
bool gps_avaliable() { return this->gps->fix; }
std::optional<Eigen::Vector2f> velocity() {
if (!this->gps.fix) {
if (!this->gps->fix) {
return std::nullopt;
}
float speed = this->gps.speed * KNOTS_TO_M_SEC;
float angle = this->gps.angle * TO_RAD;
float speed = this->gps->speed * KNOTS_TO_M_SEC;
float angle = this->gps->angle * TO_RAD;
return Eigen::Vector2f(cos(M_PI_2 - angle), sin(M_PI_2 - angle)) * speed;
}
@ -66,28 +67,36 @@ struct GPS {
float y = dLat * earth_radius;
float x = dLon * earth_radius * std::cos(meanLat);
float z = this->gps.altitude;
float z = this->gps->altitude;
return Eigen::Vector3f(x, y, z);
}
std::optional<Eigen::Vector3f> get_coordinates() {
if (this->gps.fix == false && this->gps.latitudeDegrees != 0.0 &&
this->gps.longitudeDegrees != 0.0) {
if (this->gps->fix == false || this->gps->latitudeDegrees == 0.0 ||
this->gps->longitudeDegrees != 0.0) {
return std::nullopt;
}
float latitude = this->gps.latitudeDegrees;
float longitude = this->gps.longitudeDegrees;
float latitude = this->gps->latitudeDegrees;
float longitude = this->gps->longitudeDegrees;
return this->waypoint_to_xyz(latitude, longitude, this->gps.altitude);
return this->waypoint_to_xyz(latitude, longitude, this->gps->altitude);
}
void poll() {
this->gps.read();
if (this->gps.newNMEAreceived()) {
ESP_LOGD("GPS", "NMEA LINE: %s", this->gps.lastNMEA());
if (!this->gps.parse(this->gps.lastNMEA()))
return;
while (this->gps->read()) {
if (this->gps->newNMEAreceived()) {
char *line = this->gps->lastNMEA();
// ESP_LOGI("GPS", "NMEA LINE: %s", line);
if (!this->gps->parse(line)) {
continue;
}
}
}
}
};
inline SemaphoreHandle_t gps_mutex;
inline GPS *gps = nullptr;
void gps_poll_task(void *_);

53
main/imu.cpp
View File

@ -2,68 +2,63 @@
#include "esp_log.h"
#include "esp_timer.h"
#include "freertos/idf_additions.h"
#include "hal/spi_types.h"
#include "nav.h"
#ifdef PS
#undef PS
#endif
#ifdef F
#undef F
#endif
#include <Eigen/Dense>
static const char *TAG = "IMU";
SemaphoreHandle_t imuStateMutex = NULL;
BNO08x *setup_imu(imu_state *state) {
BNO08x *imu = new BNO08x();
BNO08x *imu = new BNO08x(
bno08x_config_t(SPI2_HOST, GPIO_NUM_26, GPIO_NUM_27, GPIO_NUM_25,
GPIO_NUM_33, GPIO_NUM_36, GPIO_NUM_32, 2000000, false));
if (!imu->initialize()) {
ESP_LOGE(TAG, "BNO08x Init failure.");
return nullptr;
}
if (imuStateMutex == NULL) {
imuStateMutex = xSemaphoreCreateMutex();
}
imu->dynamic_calibration_autosave_enable();
imu->dynamic_calibration_enable(BNO08xCalSel::all);
imu->rpt.rv_game.enable(2500UL);
imu->rpt.linear_accelerometer.enable(2500UL);
// imu->rpt.rv_game.tare();
imu->register_cb([imu, state]() {
if (imu->rpt.rv_game.has_new_data()) {
auto sens_rot = imu->rpt.rv_game.get_quat();
auto euler = imu->rpt.rv_game.get_euler();
if (xSemaphoreTake(imuStateMutex, 0) == pdTRUE) {
state->euler_ang = {euler.x, euler.y, euler.z};
state->rot = {sens_rot.i, sens_rot.j, sens_rot.k, sens_rot.real};
xSemaphoreGive(imuStateMutex);
}
}
if (imu->rpt.linear_accelerometer.has_new_data()) {
if (xSemaphoreTake(imuStateMutex, 0) == pdTRUE) {
auto sens_accel = imu->rpt.linear_accelerometer.get();
state->accel = {sens_accel.x, sens_accel.y, sens_accel.z};
int64_t current_time = esp_timer_get_time();
if (state->last_time != -1) {
float dt = (current_time - state->last_time) / 1000000.0f;
float dt = ((float)(current_time - state->last_time)) / 1000000.0f;
Vec3C accel_global = apply_rot(&state->accel, &state->rot);
Vec3C delta = apply_rot(&state->accel, &state->rot);
// Trapezoidal Integration for Velocity: v = v0 + 0.5 * (a + a0) * dt
delta.x += state->last_accel.x;
delta.y += state->last_accel.y;
delta.z += state->last_accel.z;
multiply_vec_by(&delta, dt * 0.5f);
add_to_vec(&state->vel, &delta);
state->last_accel = state->accel;
if (xSemaphoreTake(nav_mutex, (TickType_t)2) == pdTRUE) {
nav_filter.predict(dt, Eigen::Vector3f(accel_global.x, accel_global.y,
accel_global.z));
xSemaphoreGive(nav_mutex);
} else {
ESP_LOGE(TAG, "Failed to get mutex.");
}
}
state->last_time = current_time;
xSemaphoreGive(imuStateMutex);
}
}
});

7
main/imu.h
View File

@ -4,19 +4,12 @@
#include <cstdint>
#include "BNO08x.hpp"
#include "BNO08xGlobalTypes.hpp"
#include "drone_controller.h"
#include "freertos/idf_additions.h"
struct imu_state {
Vec3C accel = {0, 0, 0};
Vec3C last_accel = {0, 0, 0};
QuatC rot = {0, 0, 0, 1};
Vec3C vel = {0, 0, 0};
int64_t last_time = -1;
Vec3C euler_ang = {0, 0, 0};
};
BNO08x *setup_imu(imu_state *state);
extern SemaphoreHandle_t imuStateMutex;

107
main/main.cpp
View File

@ -1,53 +1,92 @@
#include "esp32-hal.h"
#include "driver/gpio.h"
#include "drone_comms.h"
#include "esp_log.h"
#include <cmath>
#include <cstdio>
#include "freertos/FreeRTOS.h"
#include "freertos/idf_additions.h"
#include "freertos/projdefs.h"
#include "freertos/task.h"
#include "imu.h"
#include <Arduino.h>
#include "env_sens.h"
#include "gps.h"
#include "nav.h"
#include "radio.h"
static const constexpr char *TAG = "Main";
SemaphoreHandle_t gps_mutex;
GPS *gps = nullptr;
void gps_poll_task(void *_) {
while (true) {
if (xSemaphoreTake(gps_mutex, (TickType_t)10) == pdTRUE) {
gps->poll();
xSemaphoreGive(gps_mutex);
}
vTaskDelay(pdMS_TO_TICKS(50));
}
}
void gps_poll_init() {
gps_mutex = xSemaphoreCreateMutex();
if (gps_mutex != NULL) {
xTaskCreate(gps_poll_task, "Writer", 2048, NULL, 1, NULL);
}
}
static const char *TAG = "MAIN";
extern "C" void app_main(void) {
nav_mutex = xSemaphoreCreateMutex();
initArduino();
gps = new GPS();
gps_poll_init();
gpio_install_isr_service(0);
Serial.begin(115200);
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
// );
//
xTaskCreate(env_sens::baro_poll_task, "baro_poll", 8192, NULL, 1, NULL);
xTaskCreate(gps_poll_task, "gps_poll", 8192, NULL, 1, NULL);
ESP_LOGI("MAIN", "All tasks spawned. Main loop free.");
Eigen::Vector3f local_pos = {0, 0, 0};
Eigen::Vector3f local_vel = {0, 0, 0};
bool nav_data_ready = false;
while (true) {
if (xSemaphoreTake(gps_mutex, (TickType_t)10) == pdTRUE) {
ESP_LOGI(TAG, "loc -> lat: %f, long: %f, height: %f",
gps->gps.latitudeDegrees, gps->gps.longitudeDegrees,
gps->gps.altitude);
if (gps->gps_avaliable()) {
// 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 (gps_mutex && xSemaphoreTake(gps_mutex, (TickType_t)10) == pdTRUE) {
if (gps->gps_avaliable()) {
ESP_LOGI(TAG, "loc -> lat: %f, long: %f, height: %f",
gps->gps->latitudeDegrees, gps->gps->longitudeDegrees,
gps->gps->altitude);
auto D_pos_cond = gps->get_coordinates();
if (D_pos_cond.has_value()) {
auto D_pos = D_pos_cond.value();
auto D_pos = gps->get_coordinates().value();
ESP_LOGI(TAG, " -> D(pos): (%f, %f, %f)", D_pos[0], D_pos[1],
D_pos[2]);
}
}
xSemaphoreGive(gps_mutex);
}
vTaskDelay(pdMS_TO_TICKS(250));
env_sens::dbg_sens();
if (nav_mutex && xSemaphoreTake(nav_mutex, pdMS_TO_TICKS(10)) == pdTRUE) {
local_pos = nav_filter.position;
local_vel = nav_filter.velocity;
nav_data_ready = true;
xSemaphoreGive(nav_mutex); // RELEASE IMMEDIATELY
}
if (nav_data_ready) {
ESP_LOGI(TAG, "nav(pos): (%f, %f, %f)", local_pos[0], local_pos[1],
local_pos[2]);
ESP_LOGI(TAG, "nav(vel): (%f, %f, %f)", local_vel[0], local_vel[1],
local_vel[2]);
}
vTaskDelay(pdMS_TO_TICKS(1000));
}
}

98
main/nav.h Normal file
View File

@ -0,0 +1,98 @@
#pragma once
#include "freertos/idf_additions.h"
#include <cmath>
#ifdef PS
#undef PS
#endif
#ifdef F
#undef F
#endif
#include <Eigen/Dense>
inline float getYawDifference(const Eigen::Vector3f &v_gps,
const Eigen::Vector3f &v_imu) {
float yaw_gps = std::atan2(v_gps.y(), v_gps.x());
float yaw_imu = std::atan2(v_imu.y(), v_imu.x());
float delta_yaw = yaw_gps - yaw_imu;
return std::atan2(std::sin(delta_yaw), std::cos(delta_yaw));
}
struct nav_compl {
Eigen::Vector3f position = Eigen::Vector3f::Zero();
Eigen::Vector3f velocity = Eigen::Vector3f::Zero();
float yaw_offset = 0.0f;
// Time Constants per axis (X, Y, Z)
// Lower = faster tracking of GPS; Higher = smoother/more IMU trust
Eigen::Vector3f tau_gps_pos = {0.5f, 0.5f, 0.5f};
Eigen::Vector3f tau_gps_vel = {1.0f, 1.0f, INFINITY};
Eigen::Vector3f tau_baro_pos = {INFINITY, INFINITY, 5.0f};
Eigen::Vector3f tau_baro_vel = {INFINITY, INFINITY, 10.0f};
float tau_yaw = 2.0f; // Yaw remains a scalar
void predict(float dt, Eigen::Vector3f accel) {
// Rotate body-frame accel to world-frame
Eigen::Vector3f accel_rotated =
Eigen::AngleAxisf(this->yaw_offset, Eigen::Vector3f::UnitZ()) * accel;
Eigen::Vector3f next_velocity = this->velocity + (accel_rotated * dt);
// Trapezoidal integration for position
this->position += (this->velocity + next_velocity) * 0.5f * dt;
this->velocity = next_velocity;
}
void measure_gps(float dt, Eigen::Vector3f gps_pos, Eigen::Vector3f gps_vel) {
// Calculate Alpha vectors using element-wise operations
// Formula: alpha = dt / (tau + dt)
Eigen::Vector3f alpha_pos = dt / (tau_gps_pos.array() + dt);
Eigen::Vector3f alpha_vel = dt / (tau_gps_vel.array() + dt);
float alpha_yaw = dt / (tau_yaw + dt);
// 1. Position Update (Element-wise LPF)
// res = (1 - alpha) * state + alpha * measurement
this->position =
(Eigen::Vector3f::Ones() - alpha_pos).array() * this->position.array() +
alpha_pos.array() * gps_pos.array();
// 2. Yaw Correction (only if moving > 1.0 m/s)
if (gps_vel.norm() > 1.0f) {
float yaw_delta = getYawDifference(gps_vel, this->velocity);
this->yaw_offset += yaw_delta * alpha_yaw;
this->yaw_offset =
std::atan2(std::sin(this->yaw_offset), std::cos(this->yaw_offset));
}
// 3. Velocity Update (Element-wise LPF)
this->velocity =
(Eigen::Vector3f::Ones() - alpha_vel).array() * this->velocity.array() +
alpha_vel.array() * gps_vel.array();
}
void measure_baro(float dt, Eigen::Vector3f baro_pos,
Eigen::Vector3f baro_vel) {
// Calculate Alpha vectors using element-wise operations
// Formula: alpha = dt / (tau + dt)
Eigen::Vector3f alpha_pos = dt / (tau_baro_pos.array() + dt);
Eigen::Vector3f alpha_vel = dt / (tau_baro_vel.array() + dt);
this->position =
(Eigen::Vector3f::Ones() - alpha_pos).array() * this->position.array() +
alpha_pos.array() * baro_pos.array();
this->velocity =
(Eigen::Vector3f::Ones() - alpha_vel).array() * this->velocity.array() +
alpha_vel.array() * baro_vel.array();
}
};
inline SemaphoreHandle_t nav_mutex = NULL;
inline nav_compl nav_filter;

65
main/pos.h
View File

@ -1,65 +0,0 @@
#ifdef PS
#undef PS
#endif
#ifdef F
#undef F
#endif
#include <Eigen/Dense>
inline float getYawDifference(const Eigen::Vector3f &v_gps,
const Eigen::Vector3f &v_imu) {
// 1. Extract the 2D components (Project to XY plane)
float yaw_gps = std::atan2(v_gps.y(), v_gps.x());
float yaw_imu = std::atan2(v_imu.y(), v_imu.x());
// 2. Calculate the raw difference
float delta_yaw = yaw_gps - yaw_imu;
// 3. Normalize the angle to [-PI, PI]
while (delta_yaw > M_PI)
delta_yaw -= 2.0 * M_PI;
while (delta_yaw < -M_PI)
delta_yaw += 2.0 * M_PI;
return delta_yaw;
}
struct nav_compl {
Eigen::Vector3f position = Eigen::Vector3f::Zero();
Eigen::Vector3f velocity = Eigen::Vector3f::Zero();
float yaw_offset = 0.0;
float yaw_delta_weight = 0.04;
Eigen::Vector3f gps_weight_vel = Eigen::Vector3f::Constant(0.03f);
Eigen::Vector3f gps_weight_pos = Eigen::Vector3f::Constant(0.03f);
void predict(float dt, Eigen::Vector3f accel) {
Eigen::Vector3f accel_rotated =
Eigen::AngleAxisf(this->yaw_offset, Eigen::Vector3d::UnitZ()) * accel;
Eigen::Vector3f next_velocity = this->velocity + (accel_rotated * dt);
this->position += (this->velocity + next_velocity) * 0.5f * dt;
this->velocity = next_velocity;
}
void measure(Eigen::Vector3f gps_pos, Eigen::Vector3f gps_vel) {
this->position = (Eigen::Vector3f::Ones() - gps_weight_pos)
.cwiseProduct(this->position) +
gps_weight_pos.cwiseProduct(gps_pos);
if (gps_vel.norm() > 1) {
float yaw_delta = getYawDifference(gps_vel, this->velocity);
yaw_offset =
yaw_offset * (1 - yaw_delta_weight) + yaw_delta * yaw_delta_weight;
}
this->velocity = (Eigen::Vector3f::Ones() - gps_weight_vel)
.cwiseProduct(this->velocity) +
gps_weight_vel.cwiseProduct(gps_vel);
}
};

92
main/radio.cpp Normal file
View File

@ -0,0 +1,92 @@
#include "radio.h"
#include "esp32-hal-gpio.h"
#include "esp_log.h"
#include "freertos/idf_additions.h"
#include <Arduino.h>
#include <RFM69.h>
#include <SPI.h>
#include <cstdint>
#include <cstring>
#include <drone_comms.h>
#define RFM69_RST 14
#define RFM69_CS 12
#define RFM69_INT 39
#define SPI_SCLK 18
#define SPI_MISO 19
#define SPI_MOSI 23
#define FREQUENCY RF69_433MHZ
#define NODEID 1
#define NETWORKID 100
static const char *TAG = "RADIO_TASK";
#define PACKET_QUEUE_CAP 10
void radio_rx_task(void *pvParameters) {
ESP_LOGI(TAG, "Radio Task Started on Core %d", xPortGetCoreID());
packet_queue = xQueueCreate(PACKET_QUEUE_CAP, MAX_PACKET_SIZE);
controller_input_semaphore = xSemaphoreCreateMutex();
pinMode(RFM69_CS, OUTPUT);
pinMode(RFM69_INT, INPUT);
// 1. Setup SPI for this task
SPIClass vspi(VSPI);
vspi.begin(SPI_SCLK, SPI_MISO, SPI_MOSI, 34);
// 3. Hardware Reset
pinMode(RFM69_RST, OUTPUT);
digitalWrite(RFM69_RST, HIGH);
vTaskDelay(pdMS_TO_TICKS(10));
digitalWrite(RFM69_RST, LOW);
vTaskDelay(pdMS_TO_TICKS(50));
RFM69 radio(RFM69_CS, RFM69_INT, true, &vspi);
if (radio.initialize(FREQUENCY, NODEID, NETWORKID)) {
radio.setHighPower(true);
ESP_LOGI(TAG, "Radio Initialized. Version: 0x%02X", radio.readReg(0x10));
} else {
ESP_LOGE(TAG, "Radio Init FAILED! Deleting Task.");
vTaskDelete(NULL);
}
// 4. Polling Loop
while (1) {
if (radio.receiveDone()) {
// 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) {
controller_input *inp =
(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);
}
} else {
xQueueSend(packet_queue, &packet_data, portMAX_DELAY);
}
if (radio.ACKRequested()) {
radio.sendACK();
}
}
// Small delay to prevent Watchdog trigger and allow other tasks to run
// 1ms is usually enough for high-speed polling
vTaskDelay(pdMS_TO_TICKS(1));
}
}

10
main/radio.h Normal file
View File

@ -0,0 +1,10 @@
#pragma once
#include "drone_comms.h"
#include "freertos/idf_additions.h"
inline QueueHandle_t packet_queue = NULL;
inline SemaphoreHandle_t controller_input_semaphore = NULL;
inline controller_input current_controller_input;
void radio_rx_task(void *pvParameters);