diff --git a/README.md b/README.md new file mode 100644 index 0000000..e065d54 --- /dev/null +++ b/README.md @@ -0,0 +1,122 @@ +# FlightControl - ESP32 Quadcopter Flight Controller + +ESP32-C3 tabanlı, profesyonel kalitede quadcopter uçuş kontrolcüsü. + +## 📋 Özellikler + +- **Attitude Estimation**: Quaternion tabanlı EKF (Extended Kalman Filter) +- **Kontrol**: Cascaded PID (Attitude + Rate loops) +- **Motor Mixer**: X-konfigürasyon quadcopter +- **Sensörler**: MPU9250 (Gyro + Accel), AK8963 (Magnetometer) +- **Haberleşme**: BLE (Nordic UART Service), WiFi +- **OTA Update**: Web tabanlı firmware güncelleme +- **RC Input**: SBUS receiver desteği +- **Web Dashboard**: Gerçek zamanlı telemetri ve PID tuning + +## 🔧 Donanım + +| Bileşen | Model | +|---------|-------| +| MCU | ESP32-C3 Super Mini | +| IMU | MPU9250 / MPU9255 | +| Motor Driver | 4x PWM ESC | + +### Bağlantı Şeması + +``` +ESP32-C3 MPU9250 +-------- ------- +GPIO6 (SDA) ───── SDA +GPIO7 (SCL) ───── SCL +3V3 ───── VCC +GND ───── GND + +ESP32-C3 ESC (PWM) +-------- -------- +GPIO2 ───── Motor 1 (Front-Right) +GPIO3 ───── Motor 2 (Back-Right) +GPIO4 ───── Motor 3 (Back-Left) +GPIO5 ───── Motor 4 (Front-Left) +``` + +## 🚀 Kurulum + +### Gereksinimler +- ESP-IDF v5.x +- Python 3.x + +### Derleme +```bash +cd "esp32 c3 Super Mini" +idf.py set-target esp32c3 +idf.py build +idf.py flash monitor +``` + +## 📱 BLE Komutları + +"QUAD-FC" cihazına bağlanarak aşağıdaki komutları gönderebilirsiniz: + +| Komut | Açıklama | +|-------|----------| +| `ARM` | Motorları aktif et | +| `DISARM` | Motorları kapat | +| `THR=xx` | Throttle ayarla (0-100) | +| `ROLL=xx` | Roll açısı (-45 to +45) | +| `PITCH=xx` | Pitch açısı (-45 to +45) | +| `YAW=xx` | Yaw rate (-180 to +180) | +| `TEL=1/0` | Telemetri aç/kapat | +| `GET` | PID değerlerini al | +| `RATE_RP=p,i,d` | Rate PID ayarla | + +## 🌐 WiFi & OTA + +### AP Mode (Varsayılan) +- SSID: `QUAD-FC-SETUP` +- Password: `12345678` + +### OTA Update +1. WiFi'ye bağlan +2. Tarayıcıda `http://192.168.4.1/update` adresine git +3. Firmware (.bin) dosyasını yükle + +### Web Dashboard +- `http://192.168.4.1/` - Gerçek zamanlı telemetri ve kontrol + +## ⚠️ Güvenlik Uyarıları + +> **DİKKAT**: Bu bir gerçek uçuş kontrolcüsüdür! + +- ARM komutundan önce pervaneleri çıkarın +- İlk testleri masada, motorlar bağlı değilken yapın +- PID değerleri her drone için farklıdır + +## 📁 Dosya Yapısı + +``` +main/ +├── main.c # Ana uygulama +├── ekf_quat.c/h # Extended Kalman Filter +├── attitude_controller.c/h # Outer loop (angle) +├── rate_controller.c/h # Inner loop (rate) +├── pid_controller.c/h # PID algoritması +├── motor_mixer.c/h # Motor karıştırıcı +├── mpu9250.c/h # IMU driver +├── ak8963.c/h # Magnetometer driver +├── ble_tuner.c/h # BLE arayüzü +├── wifi_manager.c/h # WiFi yönetimi +├── ota_update.c/h # OTA güncelleme +├── sbus_receiver.c/h # SBUS RC input +├── web_server.c/h # Web dashboard +└── fc_error.h # Hata kodları +``` + +## 📄 Lisans + +MIT License + +## 👤 Yazar + +Emrah Duatepe ([@ZekDe](https://github.com/ZekDe)) + +*Bu proje yapay zeka yardımıyla geliştirilmiştir.* diff --git a/esp32 c3 Super Mini/main/CMakeLists.txt b/esp32 c3 Super Mini/main/CMakeLists.txt index 08fde7a..f7c1128 100644 --- a/esp32 c3 Super Mini/main/CMakeLists.txt +++ b/esp32 c3 Super Mini/main/CMakeLists.txt @@ -7,6 +7,12 @@ idf_component_register(SRCS "helper.c" "ble_tuner.c" "quaternion_math.c" "pid_controller.c" "i2c_driver.c" "mpu9250.c" + "ak8963.c" + "wifi_manager.c" + "ota_update.c" + "sbus_receiver.c" + "web_server.c" + "fc_error.c" "ton.c" "edge_detection.c" "main.c" diff --git a/esp32 c3 Super Mini/main/ak8963.c b/esp32 c3 Super Mini/main/ak8963.c new file mode 100644 index 0000000..7d0a1f6 --- /dev/null +++ b/esp32 c3 Super Mini/main/ak8963.c @@ -0,0 +1,452 @@ +/** + * @file ak8963.c + * @brief AK8963 Magnetometer Driver Implementation + */ + +#include "ak8963.h" +#include "esp_log.h" +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include +#include + +static const char *TAG = "AK8963"; + +/******************************************************************************* + * Constants + ******************************************************************************/ +#define MAG_SCALE_14BIT (0.6f) /* µT per LSB (14-bit mode) */ +#define MAG_SCALE_16BIT (0.15f) /* µT per LSB (16-bit mode) */ + +#ifndef M_PI +#define M_PI 3.14159265358979323846f +#endif + +/******************************************************************************* + * Private Functions + ******************************************************************************/ + +/** + * @brief Normalize angle to 0-360 range + */ +static float normalizeHeading(float heading) +{ + while (heading < 0.0f) heading += 360.0f; + while (heading >= 360.0f) heading -= 360.0f; + return heading; +} + +/******************************************************************************* + * Bypass Mode Control + ******************************************************************************/ + +int8_t ak8963EnableBypass(i2c_master_dev_handle_t mpu_dev) +{ + if (mpu_dev == NULL) { + return -1; + } + + int8_t ret; + + /* + * To access AK8963, we need to enable I2C bypass mode on MPU9250. + * This connects the auxiliary I2C bus directly to the main I2C bus. + * + * INT_PIN_CFG register (0x37): + * Bit 1 (BYPASS_EN) = 1: Enable bypass mode + * Bit 7 (ACTL) = 0: INT active high (default) + * Bit 6 (OPEN) = 0: INT push-pull (default) + * Bit 5 (LATCH_INT_EN) = 0: INT pulse (default) + * Bit 4 (INT_ANYRD_2CLEAR) = 1: INT cleared on any read + * + * Value = 0x12 (bypass enabled, int cleared on any read) + */ + + /* First, disable I2C master mode */ + ret = i2cWriteByte(mpu_dev, MPU9250_REG_USER_CTRL, 0x00); + if (ret != 0) { + ESP_LOGE(TAG, "Failed to disable I2C master mode"); + return -2; + } + + vTaskDelay(pdMS_TO_TICKS(10)); + + /* Enable bypass mode */ + ret = i2cWriteByte(mpu_dev, MPU9250_REG_INT_PIN_CFG, 0x02); + if (ret != 0) { + ESP_LOGE(TAG, "Failed to enable bypass mode"); + return -3; + } + + vTaskDelay(pdMS_TO_TICKS(10)); + + ESP_LOGI(TAG, "MPU9250 bypass mode enabled"); + return 0; +} + +/******************************************************************************* + * Initialization + ******************************************************************************/ + +void ak8963GetDefaultConfig(ak8963_config_t *config) +{ + if (config == NULL) return; + + config->mode = AK8963_MODE_CONT2; /* Continuous 100Hz */ + config->output = AK8963_OUTPUT_16BIT; /* 16-bit resolution */ +} + +int8_t ak8963Init(ak8963_handle_t *handle, + i2c_driver_t *i2c_driver, + i2c_master_dev_handle_t mpu_dev, + const ak8963_config_t *config) +{ + if (handle == NULL || i2c_driver == NULL || config == NULL) { + ESP_LOGE(TAG, "Invalid parameters"); + return -1; + } + + int8_t ret; + memset(handle, 0, sizeof(ak8963_handle_t)); + + /* Store MPU device handle for bypass control */ + handle->mpu_dev = mpu_dev; + + /* Enable bypass mode first */ + ret = ak8963EnableBypass(mpu_dev); + if (ret != 0) { + return -2; + } + + /* Add AK8963 to I2C bus */ + ret = i2cDriverAddDevice(i2c_driver, AK8963_I2C_ADDR, &handle->i2c_dev); + if (ret != 0) { + ESP_LOGE(TAG, "Failed to add AK8963 to I2C bus"); + return -3; + } + + /* Verify device by reading WHO_AM_I */ + uint8_t who_am_i; + ret = i2cReadByte(handle->i2c_dev, AK8963_REG_WIA, &who_am_i); + if (ret != 0) { + ESP_LOGE(TAG, "Failed to read WHO_AM_I"); + i2cDriverRemoveDevice(handle->i2c_dev); + return -4; + } + + if (who_am_i != AK8963_WHO_AM_I_VALUE) { + ESP_LOGE(TAG, "Invalid WHO_AM_I: 0x%02X (expected 0x%02X)", + who_am_i, AK8963_WHO_AM_I_VALUE); + i2cDriverRemoveDevice(handle->i2c_dev); + return -5; + } + + ESP_LOGI(TAG, "AK8963 detected (WHO_AM_I: 0x%02X)", who_am_i); + + /* Soft reset */ + ret = ak8963Reset(handle); + if (ret != 0) { + i2cDriverRemoveDevice(handle->i2c_dev); + return -6; + } + + vTaskDelay(pdMS_TO_TICKS(100)); + + /* Read sensitivity adjustment values from Fuse ROM */ + ret = i2cWriteByte(handle->i2c_dev, AK8963_REG_CNTL1, AK8963_MODE_FUSE_ROM); + if (ret != 0) { + ESP_LOGE(TAG, "Failed to enter Fuse ROM mode"); + i2cDriverRemoveDevice(handle->i2c_dev); + return -7; + } + + vTaskDelay(pdMS_TO_TICKS(10)); + + uint8_t asa[3]; + ret = i2cReadBytes(handle->i2c_dev, AK8963_REG_ASAX, asa, 3); + if (ret != 0) { + ESP_LOGE(TAG, "Failed to read sensitivity adjustment"); + i2cDriverRemoveDevice(handle->i2c_dev); + return -8; + } + + /* + * Calculate sensitivity adjustment factors + * Formula from datasheet: Hadj = H * ((ASA - 128) * 0.5 / 128 + 1) + * Simplified: Hadj = H * (ASA - 128) / 256 + H + */ + handle->asa_x = (float)(asa[0] - 128) / 256.0f + 1.0f; + handle->asa_y = (float)(asa[1] - 128) / 256.0f + 1.0f; + handle->asa_z = (float)(asa[2] - 128) / 256.0f + 1.0f; + + ESP_LOGI(TAG, "Sensitivity adjustment: X=%.3f Y=%.3f Z=%.3f", + handle->asa_x, handle->asa_y, handle->asa_z); + + /* Power down before mode change */ + ret = i2cWriteByte(handle->i2c_dev, AK8963_REG_CNTL1, AK8963_MODE_POWER_DOWN); + if (ret != 0) { + i2cDriverRemoveDevice(handle->i2c_dev); + return -9; + } + + vTaskDelay(pdMS_TO_TICKS(10)); + + /* Set operating mode */ + uint8_t cntl1 = (uint8_t)config->mode | (uint8_t)config->output; + ret = i2cWriteByte(handle->i2c_dev, AK8963_REG_CNTL1, cntl1); + if (ret != 0) { + ESP_LOGE(TAG, "Failed to set operating mode"); + i2cDriverRemoveDevice(handle->i2c_dev); + return -10; + } + + vTaskDelay(pdMS_TO_TICKS(10)); + + /* Store configuration */ + handle->config = *config; + + /* Set scale factor based on output bit setting */ + handle->mag_scale = (config->output == AK8963_OUTPUT_16BIT) ? + MAG_SCALE_16BIT : MAG_SCALE_14BIT; + + /* Set default calibration (no correction) */ + ak8963GetDefaultCalibration(&handle->calibration); + + handle->is_initialized = 1; + + ESP_LOGI(TAG, "AK8963 initialized successfully"); + ESP_LOGI(TAG, " Mode: %s, Resolution: %d-bit", + (config->mode == AK8963_MODE_CONT2) ? "Continuous 100Hz" : "Other", + (config->output == AK8963_OUTPUT_16BIT) ? 16 : 14); + + return 0; +} + +int8_t ak8963Deinit(ak8963_handle_t *handle) +{ + if (handle == NULL) return -1; + + if (!handle->is_initialized) return 0; + + /* Power down */ + i2cWriteByte(handle->i2c_dev, AK8963_REG_CNTL1, AK8963_MODE_POWER_DOWN); + + /* Remove from I2C bus */ + i2cDriverRemoveDevice(handle->i2c_dev); + + handle->is_initialized = 0; + + ESP_LOGI(TAG, "AK8963 deinitialized"); + return 0; +} + +int8_t ak8963Reset(ak8963_handle_t *handle) +{ + if (handle == NULL) return -1; + + /* Soft reset: CNTL2 bit 0 = 1 */ + return i2cWriteByte(handle->i2c_dev, AK8963_REG_CNTL2, 0x01); +} + +/******************************************************************************* + * Data Reading + ******************************************************************************/ + +int8_t ak8963DataReady(ak8963_handle_t *handle, uint8_t *ready) +{ + if (handle == NULL || ready == NULL) return -1; + if (!handle->is_initialized) return -2; + + uint8_t st1; + int8_t ret = i2cReadByte(handle->i2c_dev, AK8963_REG_ST1, &st1); + if (ret != 0) return -3; + + *ready = (st1 & 0x01) ? 1 : 0; /* DRDY bit */ + + return 0; +} + +int8_t ak8963ReadRaw(ak8963_handle_t *handle, ak8963_raw_data_t *raw_data) +{ + if (handle == NULL || raw_data == NULL) return -1; + if (!handle->is_initialized) return -2; + + /* Read ST1 through ST2 (7 bytes total) */ + uint8_t buffer[7]; + int8_t ret = i2cReadBytes(handle->i2c_dev, AK8963_REG_ST1, buffer, 7); + if (ret != 0) return -3; + + raw_data->st1 = buffer[0]; + + /* Little-endian data */ + raw_data->hx = (int16_t)((buffer[2] << 8) | buffer[1]); + raw_data->hy = (int16_t)((buffer[4] << 8) | buffer[3]); + raw_data->hz = (int16_t)((buffer[6] << 8) | buffer[5]); + + /* ST2 must be read to signal end of measurement */ + ret = i2cReadByte(handle->i2c_dev, AK8963_REG_ST2, &raw_data->st2); + if (ret != 0) return -4; + + /* Check for magnetic overflow */ + if (raw_data->st2 & 0x08) { + ESP_LOGW(TAG, "Magnetic sensor overflow!"); + return -5; + } + + return 0; +} + +int8_t ak8963ReadScaled(ak8963_handle_t *handle, ak8963_scaled_data_t *scaled_data) +{ + if (handle == NULL || scaled_data == NULL) return -1; + + ak8963_raw_data_t raw; + int8_t ret = ak8963ReadRaw(handle, &raw); + if (ret != 0) return ret; + + /* Apply sensitivity adjustment and scale to µT */ + scaled_data->mx = (float)raw.hx * handle->mag_scale * handle->asa_x; + scaled_data->my = (float)raw.hy * handle->mag_scale * handle->asa_y; + scaled_data->mz = (float)raw.hz * handle->mag_scale * handle->asa_z; + + /* Calculate simple heading (no tilt compensation) */ + scaled_data->heading = ak8963CalculateHeading(scaled_data->mx, + scaled_data->my, 0.0f); + + return 0; +} + +int8_t ak8963ReadCalibrated(ak8963_handle_t *handle, ak8963_scaled_data_t *scaled_data) +{ + if (handle == NULL || scaled_data == NULL) return -1; + if (!handle->is_initialized) return -2; + + /* First read scaled (uncalibrated) data */ + int8_t ret = ak8963ReadScaled(handle, scaled_data); + if (ret != 0) return ret; + + /* Apply calibration if available */ + if (handle->calibration.is_calibrated) { + /* Remove hard iron offset */ + float mx = scaled_data->mx - handle->calibration.offset_x; + float my = scaled_data->my - handle->calibration.offset_y; + float mz = scaled_data->mz - handle->calibration.offset_z; + + /* Apply soft iron correction (scale) */ + scaled_data->mx = mx * handle->calibration.scale_x; + scaled_data->my = my * handle->calibration.scale_y; + scaled_data->mz = mz * handle->calibration.scale_z; + + /* Recalculate heading with calibrated data */ + scaled_data->heading = ak8963CalculateHeading(scaled_data->mx, + scaled_data->my, 0.0f); + } + + return 0; +} + +/******************************************************************************* + * Calibration + ******************************************************************************/ + +void ak8963GetDefaultCalibration(ak8963_calibration_t *calibration) +{ + if (calibration == NULL) return; + + memset(calibration, 0, sizeof(ak8963_calibration_t)); + + /* No offset correction */ + calibration->offset_x = 0.0f; + calibration->offset_y = 0.0f; + calibration->offset_z = 0.0f; + + /* Unity scale (no scaling) */ + calibration->scale_x = 1.0f; + calibration->scale_y = 1.0f; + calibration->scale_z = 1.0f; + + /* Identity rotation matrix */ + calibration->rotation[0] = 1.0f; + calibration->rotation[4] = 1.0f; + calibration->rotation[8] = 1.0f; + + calibration->is_calibrated = 0; +} + +int8_t ak8963SetCalibration(ak8963_handle_t *handle, + const ak8963_calibration_t *calibration) +{ + if (handle == NULL || calibration == NULL) return -1; + if (!handle->is_initialized) return -2; + + handle->calibration = *calibration; + + ESP_LOGI(TAG, "Calibration set:"); + ESP_LOGI(TAG, " Offsets: X=%.2f Y=%.2f Z=%.2f µT", + calibration->offset_x, calibration->offset_y, calibration->offset_z); + ESP_LOGI(TAG, " Scales: X=%.3f Y=%.3f Z=%.3f", + calibration->scale_x, calibration->scale_y, calibration->scale_z); + + return 0; +} + +/******************************************************************************* + * Heading Calculation + ******************************************************************************/ + +float ak8963CalculateHeading(float mx, float my, float declination) +{ + /* + * Calculate heading from magnetometer X and Y + * Heading = atan2(my, mx) converted to degrees + * + * Note: This is only valid when the device is level! + * For accurate heading when tilted, use tilt-compensated version. + */ + + float heading_rad = atan2f(my, mx); + float heading_deg = heading_rad * 180.0f / M_PI; + + /* Add magnetic declination */ + heading_deg += declination; + + return normalizeHeading(heading_deg); +} + +float ak8963CalculateTiltCompensatedHeading(float mx, float my, float mz, + float roll, float pitch, + float declination) +{ + /* + * Tilt-compensated heading calculation + * + * When the device is tilted, the raw magnetometer reading does not + * represent the horizontal magnetic field component. We need to + * project the magnetic vector onto the horizontal plane. + * + * Using rotation matrix to compensate: + * mx_h = mx*cos(pitch) + my*sin(roll)*sin(pitch) + mz*cos(roll)*sin(pitch) + * my_h = my*cos(roll) - mz*sin(roll) + */ + + float cos_roll = cosf(roll); + float sin_roll = sinf(roll); + float cos_pitch = cosf(pitch); + float sin_pitch = sinf(pitch); + + /* Horizontal components */ + float mx_h = mx * cos_pitch + + my * sin_roll * sin_pitch + + mz * cos_roll * sin_pitch; + + float my_h = my * cos_roll - mz * sin_roll; + + /* Calculate heading */ + float heading_rad = atan2f(my_h, mx_h); + float heading_deg = heading_rad * 180.0f / M_PI; + + /* Add magnetic declination */ + heading_deg += declination; + + return normalizeHeading(heading_deg); +} diff --git a/esp32 c3 Super Mini/main/ak8963.h b/esp32 c3 Super Mini/main/ak8963.h new file mode 100644 index 0000000..1b43944 --- /dev/null +++ b/esp32 c3 Super Mini/main/ak8963.h @@ -0,0 +1,276 @@ +/** + * @file ak8963.h + * @brief AK8963 Magnetometer Driver for MPU9250 + * @details 3-axis magnetometer embedded in MPU9250/9255 + * + * @note Accessed via MPU9250 I2C bypass mode + * @note Full scale: ±4912 µT (14-bit) or ±4912 µT (16-bit) + */ + +#ifndef AK8963_H +#define AK8963_H + +#include +#include "i2c_driver.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/******************************************************************************* + * AK8963 I2C Address + ******************************************************************************/ +#define AK8963_I2C_ADDR 0x0C /* Default I2C address */ +#define AK8963_WHO_AM_I_VALUE 0x48 /* Device ID */ + +/******************************************************************************* + * AK8963 Register Map + ******************************************************************************/ +#define AK8963_REG_WIA 0x00 /* Device ID (should return 0x48) */ +#define AK8963_REG_INFO 0x01 /* Device information */ +#define AK8963_REG_ST1 0x02 /* Status 1 (data ready) */ +#define AK8963_REG_HXL 0x03 /* X-axis data LSB */ +#define AK8963_REG_HXH 0x04 /* X-axis data MSB */ +#define AK8963_REG_HYL 0x05 /* Y-axis data LSB */ +#define AK8963_REG_HYH 0x06 /* Y-axis data MSB */ +#define AK8963_REG_HZL 0x07 /* Z-axis data LSB */ +#define AK8963_REG_HZH 0x08 /* Z-axis data MSB */ +#define AK8963_REG_ST2 0x09 /* Status 2 (overflow, output setting) */ +#define AK8963_REG_CNTL1 0x0A /* Control 1 (mode, output bit) */ +#define AK8963_REG_CNTL2 0x0B /* Control 2 (soft reset) */ +#define AK8963_REG_ASTC 0x0C /* Self-test control */ +#define AK8963_REG_TS1 0x0D /* Test 1 (factory use) */ +#define AK8963_REG_TS2 0x0E /* Test 2 (factory use) */ +#define AK8963_REG_I2CDIS 0x0F /* I2C disable */ +#define AK8963_REG_ASAX 0x10 /* Sensitivity adjustment X */ +#define AK8963_REG_ASAY 0x11 /* Sensitivity adjustment Y */ +#define AK8963_REG_ASAZ 0x12 /* Sensitivity adjustment Z */ + +/******************************************************************************* + * MPU9250 Registers for Bypass Mode + ******************************************************************************/ +#define MPU9250_REG_INT_PIN_CFG 0x37 /* INT Pin/Bypass Enable Config */ +#define MPU9250_REG_USER_CTRL 0x6A /* User Control */ + +/******************************************************************************* + * AK8963 Operation Modes + ******************************************************************************/ +typedef enum { + AK8963_MODE_POWER_DOWN = 0x00, /* Power-down mode */ + AK8963_MODE_SINGLE = 0x01, /* Single measurement mode */ + AK8963_MODE_CONT1 = 0x02, /* Continuous 1 (8Hz) */ + AK8963_MODE_CONT2 = 0x06, /* Continuous 2 (100Hz) */ + AK8963_MODE_EXT_TRIG = 0x04, /* External trigger mode */ + AK8963_MODE_SELF_TEST = 0x08, /* Self-test mode */ + AK8963_MODE_FUSE_ROM = 0x0F, /* Fuse ROM access mode */ +} ak8963_mode_t; + +/******************************************************************************* + * AK8963 Output Bit Setting + ******************************************************************************/ +typedef enum { + AK8963_OUTPUT_14BIT = 0x00, /* 14-bit output (0.6 µT/LSB) */ + AK8963_OUTPUT_16BIT = 0x10, /* 16-bit output (0.15 µT/LSB) */ +} ak8963_output_t; + +/******************************************************************************* + * Calibration Data Structure + ******************************************************************************/ +typedef struct { + /* Hard iron offset (µT) */ + float offset_x; + float offset_y; + float offset_z; + + /* Soft iron scale factors */ + float scale_x; + float scale_y; + float scale_z; + + /* Rotation matrix for misalignment (optional) */ + float rotation[9]; /* 3x3 row-major */ + + uint8_t is_calibrated; +} ak8963_calibration_t; + +/******************************************************************************* + * Raw Data Structure + ******************************************************************************/ +typedef struct { + int16_t hx; /* X-axis raw */ + int16_t hy; /* Y-axis raw */ + int16_t hz; /* Z-axis raw */ + uint8_t st1; /* Status 1 */ + uint8_t st2; /* Status 2 */ +} ak8963_raw_data_t; + +/******************************************************************************* + * Scaled Data Structure + ******************************************************************************/ +typedef struct { + float mx; /* X-axis [µT] */ + float my; /* Y-axis [µT] */ + float mz; /* Z-axis [µT] */ + float heading; /* Magnetic heading [degrees] (0-360) */ +} ak8963_scaled_data_t; + +/******************************************************************************* + * Configuration Structure + ******************************************************************************/ +typedef struct { + ak8963_mode_t mode; + ak8963_output_t output; +} ak8963_config_t; + +/******************************************************************************* + * Device Handle + ******************************************************************************/ +typedef struct { + i2c_master_dev_handle_t i2c_dev; + i2c_master_dev_handle_t mpu_dev; /* MPU9250 device for bypass control */ + + ak8963_config_t config; + ak8963_calibration_t calibration; + + /* Sensitivity adjustment values from Fuse ROM */ + float asa_x; /* Sensitivity adjustment X */ + float asa_y; /* Sensitivity adjustment Y */ + float asa_z; /* Sensitivity adjustment Z */ + + /* Scale factor: LSB to µT */ + float mag_scale; + + uint8_t is_initialized; +} ak8963_handle_t; + +/******************************************************************************* + * Function Prototypes - Initialization + ******************************************************************************/ + +/** + * @brief Enable I2C bypass mode on MPU9250 + * @param mpu_dev MPU9250 I2C device handle + * @return 0 on success, negative on error + * + * @note Must be called before AK8963 can be accessed + */ +int8_t ak8963EnableBypass(i2c_master_dev_handle_t mpu_dev); + +/** + * @brief Initialize AK8963 magnetometer + * @param handle Pointer to AK8963 handle + * @param i2c_driver Pointer to I2C driver + * @param mpu_dev MPU9250 device handle (for bypass control) + * @param config Configuration settings + * @return 0 on success, negative on error + */ +int8_t ak8963Init(ak8963_handle_t *handle, + i2c_driver_t *i2c_driver, + i2c_master_dev_handle_t mpu_dev, + const ak8963_config_t *config); + +/** + * @brief Get default configuration (Continuous 100Hz, 16-bit) + * @param config Pointer to store default config + */ +void ak8963GetDefaultConfig(ak8963_config_t *config); + +/** + * @brief Deinitialize AK8963 + * @param handle Pointer to AK8963 handle + * @return 0 on success, negative on error + */ +int8_t ak8963Deinit(ak8963_handle_t *handle); + +/** + * @brief Soft reset AK8963 + * @param handle Pointer to AK8963 handle + * @return 0 on success, negative on error + */ +int8_t ak8963Reset(ak8963_handle_t *handle); + +/******************************************************************************* + * Function Prototypes - Data Reading + ******************************************************************************/ + +/** + * @brief Check if new data is ready + * @param handle Pointer to AK8963 handle + * @param ready Pointer to store result (1 = ready) + * @return 0 on success, negative on error + */ +int8_t ak8963DataReady(ak8963_handle_t *handle, uint8_t *ready); + +/** + * @brief Read raw magnetometer data + * @param handle Pointer to AK8963 handle + * @param raw_data Pointer to store raw data + * @return 0 on success, negative on error + */ +int8_t ak8963ReadRaw(ak8963_handle_t *handle, ak8963_raw_data_t *raw_data); + +/** + * @brief Read and convert to scaled data (µT) + * @param handle Pointer to AK8963 handle + * @param scaled_data Pointer to store scaled data + * @return 0 on success, negative on error + */ +int8_t ak8963ReadScaled(ak8963_handle_t *handle, ak8963_scaled_data_t *scaled_data); + +/** + * @brief Read calibrated magnetometer data + * @param handle Pointer to AK8963 handle + * @param scaled_data Pointer to store calibrated data + * @return 0 on success, negative on error + * + * @note Applies hard/soft iron calibration if available + */ +int8_t ak8963ReadCalibrated(ak8963_handle_t *handle, ak8963_scaled_data_t *scaled_data); + +/******************************************************************************* + * Function Prototypes - Calibration + ******************************************************************************/ + +/** + * @brief Set calibration parameters + * @param handle Pointer to AK8963 handle + * @param calibration Pointer to calibration data + * @return 0 on success, negative on error + */ +int8_t ak8963SetCalibration(ak8963_handle_t *handle, + const ak8963_calibration_t *calibration); + +/** + * @brief Get default calibration (no correction) + * @param calibration Pointer to store default calibration + */ +void ak8963GetDefaultCalibration(ak8963_calibration_t *calibration); + +/** + * @brief Calculate heading from magnetometer data + * @param mx X-axis magnetic field [µT] + * @param my Y-axis magnetic field [µT] + * @param declination Magnetic declination [degrees] (optional, pass 0) + * @return Heading in degrees (0-360) + */ +float ak8963CalculateHeading(float mx, float my, float declination); + +/** + * @brief Calculate tilt-compensated heading + * @param mx X-axis magnetic field [µT] + * @param my Y-axis magnetic field [µT] + * @param mz Z-axis magnetic field [µT] + * @param roll Roll angle [rad] + * @param pitch Pitch angle [rad] + * @param declination Magnetic declination [degrees] + * @return Tilt-compensated heading in degrees (0-360) + */ +float ak8963CalculateTiltCompensatedHeading(float mx, float my, float mz, + float roll, float pitch, + float declination); + +#ifdef __cplusplus +} +#endif + +#endif /* AK8963_H */ diff --git a/esp32 c3 Super Mini/main/fc_error.c b/esp32 c3 Super Mini/main/fc_error.c new file mode 100644 index 0000000..c5812a7 --- /dev/null +++ b/esp32 c3 Super Mini/main/fc_error.c @@ -0,0 +1,65 @@ +/** + * @file fc_error.c + * @brief Error code string implementations + */ + +#include "fc_error.h" + +const char* fc_error_str(fc_error_t err) +{ + switch (err) { + case FC_OK: return "Success"; + + /* Generic */ + case FC_ERR_NULL_PTR: return "Null pointer"; + case FC_ERR_INVALID_PARAM: return "Invalid parameter"; + case FC_ERR_NOT_INITIALIZED:return "Not initialized"; + case FC_ERR_ALREADY_INIT: return "Already initialized"; + case FC_ERR_TIMEOUT: return "Timeout"; + case FC_ERR_BUSY: return "Busy"; + case FC_ERR_NO_MEMORY: return "Out of memory"; + case FC_ERR_NOT_SUPPORTED: return "Not supported"; + case FC_ERR_UNKNOWN: return "Unknown error"; + + /* I2C */ + case FC_ERR_I2C_INIT: return "I2C init failed"; + case FC_ERR_I2C_TIMEOUT: return "I2C timeout"; + case FC_ERR_I2C_NACK: return "I2C NACK"; + case FC_ERR_I2C_WRITE: return "I2C write failed"; + case FC_ERR_I2C_READ: return "I2C read failed"; + case FC_ERR_I2C_BUS: return "I2C bus error"; + case FC_ERR_I2C_BUFFER: return "I2C buffer overflow"; + + /* Sensor */ + case FC_ERR_SENSOR_NOT_FOUND: return "Sensor not found"; + case FC_ERR_SENSOR_WHO_AM_I: return "Wrong WHO_AM_I"; + case FC_ERR_SENSOR_CONFIG: return "Sensor config failed"; + case FC_ERR_SENSOR_CALIB: return "Calibration failed"; + case FC_ERR_SENSOR_READ: return "Sensor read failed"; + case FC_ERR_SENSOR_OVERFLOW: return "Sensor overflow"; + + /* Control */ + case FC_ERR_CTRL_NOT_INIT: return "Controller not init"; + case FC_ERR_CTRL_SATURATED: return "Controller saturated"; + case FC_ERR_CTRL_DIVERGED: return "Controller diverged"; + case FC_ERR_CTRL_MATRIX: return "Matrix op failed"; + case FC_ERR_CTRL_SINGULAR: return "Singular matrix"; + case FC_ERR_CTRL_COMPUTE: return "Computation error"; + + /* Communication */ + case FC_ERR_COMM_DISCONN: return "Disconnected"; + case FC_ERR_COMM_TIMEOUT: return "Comm timeout"; + case FC_ERR_COMM_PARSE: return "Parse error"; + case FC_ERR_COMM_CRC: return "CRC error"; + case FC_ERR_COMM_AUTH: return "Auth failed"; + case FC_ERR_COMM_ENCRYPT: return "Encryption error"; + + /* Storage */ + case FC_ERR_NVS_OPEN: return "NVS open failed"; + case FC_ERR_NVS_READ: return "NVS read failed"; + case FC_ERR_NVS_WRITE: return "NVS write failed"; + case FC_ERR_NVS_ERASE: return "NVS erase failed"; + + default: return "Unknown"; + } +} diff --git a/esp32 c3 Super Mini/main/fc_error.h b/esp32 c3 Super Mini/main/fc_error.h new file mode 100644 index 0000000..ad6c331 --- /dev/null +++ b/esp32 c3 Super Mini/main/fc_error.h @@ -0,0 +1,131 @@ +/** + * @file fc_error.h + * @brief Centralized error codes for Flight Controller + * @details Unified error handling across all modules + */ + +#ifndef FC_ERROR_H +#define FC_ERROR_H + +#ifdef __cplusplus +extern "C" { +#endif + +/******************************************************************************* + * Error Code Categories + * + * 0: Success + * -1 to -9: Generic errors + * -10 to -19: I2C errors + * -20 to -29: Sensor errors + * -30 to -39: Control errors + * -40 to -49: Communication errors + * -50 to -59: Storage errors + ******************************************************************************/ + +typedef enum { + /* Success */ + FC_OK = 0, + + /* Generic errors (-1 to -9) */ + FC_ERR_NULL_PTR = -1, /**< Null pointer passed */ + FC_ERR_INVALID_PARAM = -2, /**< Invalid parameter */ + FC_ERR_NOT_INITIALIZED = -3, /**< Module not initialized */ + FC_ERR_ALREADY_INIT = -4, /**< Module already initialized */ + FC_ERR_TIMEOUT = -5, /**< Operation timed out */ + FC_ERR_BUSY = -6, /**< Resource busy */ + FC_ERR_NO_MEMORY = -7, /**< Out of memory */ + FC_ERR_NOT_SUPPORTED = -8, /**< Operation not supported */ + FC_ERR_UNKNOWN = -9, /**< Unknown error */ + + /* I2C errors (-10 to -19) */ + FC_ERR_I2C_INIT = -10, /**< I2C initialization failed */ + FC_ERR_I2C_TIMEOUT = -11, /**< I2C timeout */ + FC_ERR_I2C_NACK = -12, /**< I2C device not responding */ + FC_ERR_I2C_WRITE = -13, /**< I2C write failed */ + FC_ERR_I2C_READ = -14, /**< I2C read failed */ + FC_ERR_I2C_BUS = -15, /**< I2C bus error */ + FC_ERR_I2C_BUFFER = -16, /**< I2C buffer overflow */ + + /* Sensor errors (-20 to -29) */ + FC_ERR_SENSOR_NOT_FOUND = -20, /**< Sensor not detected */ + FC_ERR_SENSOR_WHO_AM_I = -21, /**< Wrong WHO_AM_I value */ + FC_ERR_SENSOR_CONFIG = -22, /**< Sensor configuration failed */ + FC_ERR_SENSOR_CALIB = -23, /**< Sensor calibration failed */ + FC_ERR_SENSOR_READ = -24, /**< Sensor read failed */ + FC_ERR_SENSOR_OVERFLOW = -25, /**< Sensor data overflow */ + + /* Control errors (-30 to -39) */ + FC_ERR_CTRL_NOT_INIT = -30, /**< Controller not initialized */ + FC_ERR_CTRL_SATURATED = -31, /**< Controller output saturated */ + FC_ERR_CTRL_DIVERGED = -32, /**< Controller diverged */ + FC_ERR_CTRL_MATRIX = -33, /**< Matrix operation failed */ + FC_ERR_CTRL_SINGULAR = -34, /**< Singular matrix (inversion failed) */ + FC_ERR_CTRL_COMPUTE = -35, /**< Computation error */ + + /* Communication errors (-40 to -49) */ + FC_ERR_COMM_DISCONN = -40, /**< Communication disconnected */ + FC_ERR_COMM_TIMEOUT = -41, /**< Communication timeout */ + FC_ERR_COMM_PARSE = -42, /**< Parse error */ + FC_ERR_COMM_CRC = -43, /**< CRC error */ + FC_ERR_COMM_AUTH = -44, /**< Authentication failed */ + FC_ERR_COMM_ENCRYPT = -45, /**< Encryption error */ + + /* Storage errors (-50 to -59) */ + FC_ERR_NVS_OPEN = -50, /**< NVS open failed */ + FC_ERR_NVS_READ = -51, /**< NVS read failed */ + FC_ERR_NVS_WRITE = -52, /**< NVS write failed */ + FC_ERR_NVS_ERASE = -53, /**< NVS erase failed */ + +} fc_error_t; + +/******************************************************************************* + * Helper Functions + ******************************************************************************/ + +/** + * @brief Check if error code is success + * @param err Error code + * @return 1 if success, 0 otherwise + */ +static inline int fc_is_ok(fc_error_t err) { + return (err == FC_OK); +} + +/** + * @brief Check if error code is an error + * @param err Error code + * @return 1 if error, 0 if success + */ +static inline int fc_is_error(fc_error_t err) { + return (err != FC_OK); +} + +/** + * @brief Get error category + * @param err Error code + * @return Category string + */ +static inline const char* fc_error_category(fc_error_t err) { + if (err == FC_OK) return "OK"; + if (err >= -9) return "GENERIC"; + if (err >= -19) return "I2C"; + if (err >= -29) return "SENSOR"; + if (err >= -39) return "CONTROL"; + if (err >= -49) return "COMM"; + if (err >= -59) return "STORAGE"; + return "UNKNOWN"; +} + +/** + * @brief Get error description string + * @param err Error code + * @return Description string + */ +const char* fc_error_str(fc_error_t err); + +#ifdef __cplusplus +} +#endif + +#endif /* FC_ERROR_H */ diff --git a/esp32 c3 Super Mini/main/i2c_driver.c b/esp32 c3 Super Mini/main/i2c_driver.c index 6b71bc7..aef93ab 100644 --- a/esp32 c3 Super Mini/main/i2c_driver.c +++ b/esp32 c3 Super Mini/main/i2c_driver.c @@ -182,6 +182,8 @@ int8_t i2cReadBytes(i2c_master_dev_handle_t dev_handle, uint8_t reg_addr, /* ============================================================================ * Write Multiple Bytes to Register * ============================================================================ */ +#define I2C_MAX_WRITE_LEN 64 /* Maximum write buffer size */ + int8_t i2cWriteBytes(i2c_master_dev_handle_t dev_handle, uint8_t reg_addr, uint8_t *data, uint16_t len) { @@ -189,8 +191,14 @@ int8_t i2cWriteBytes(i2c_master_dev_handle_t dev_handle, uint8_t reg_addr, return -1; } - /* Create buffer with register address + data */ - uint8_t write_buf[len + 1]; + /* Check buffer size limit to prevent stack overflow */ + if (len + 1 > I2C_MAX_WRITE_LEN) { + ESP_LOGE(TAG, "Write length %d exceeds max %d", len, I2C_MAX_WRITE_LEN - 1); + return -3; + } + + /* Create buffer with register address + data (static size) */ + uint8_t write_buf[I2C_MAX_WRITE_LEN]; write_buf[0] = reg_addr; memcpy(&write_buf[1], data, len); diff --git a/esp32 c3 Super Mini/main/ota_update.c b/esp32 c3 Super Mini/main/ota_update.c new file mode 100644 index 0000000..775f635 --- /dev/null +++ b/esp32 c3 Super Mini/main/ota_update.c @@ -0,0 +1,299 @@ +/** + * @file ota_update.c + * @brief OTA Update Implementation + */ + +#include "ota_update.h" +#include "esp_ota_ops.h" +#include "esp_http_server.h" +#include "esp_log.h" +#include "esp_system.h" +#include + +static const char *TAG = "ota"; + +/******************************************************************************* + * Private Variables + ******************************************************************************/ +static httpd_handle_t s_server = NULL; +static ota_status_t s_status = OTA_STATUS_IDLE; +static uint32_t s_downloaded = 0; +static uint32_t s_total = 0; + +static ota_progress_cb_t s_progress_cb = NULL; +static ota_complete_cb_t s_complete_cb = NULL; + +/******************************************************************************* + * HTML Page for Upload + ******************************************************************************/ +static const char *OTA_HTML = +"" +"OTA Update" +"" +"" +"
" +"

Flight Controller OTA

" +"
" +"" +"" +"
" +"
" +"
Ready for upload
" +"
" +""; + +/******************************************************************************* + * HTTP Handlers + ******************************************************************************/ + +/* GET /update - Serve upload page */ +static esp_err_t otaPageHandler(httpd_req_t *req) +{ + httpd_resp_set_type(req, "text/html"); + httpd_resp_send(req, OTA_HTML, strlen(OTA_HTML)); + return ESP_OK; +} + +/* POST /update - Handle firmware upload */ +static esp_err_t otaUploadHandler(httpd_req_t *req) +{ + esp_ota_handle_t ota_handle; + const esp_partition_t *update_partition; + esp_err_t err; + + s_status = OTA_STATUS_DOWNLOADING; + s_downloaded = 0; + s_total = req->content_len; + + ESP_LOGI(TAG, "Starting OTA, size: %lu bytes", (unsigned long)s_total); + + /* Get update partition */ + update_partition = esp_ota_get_next_update_partition(NULL); + if (update_partition == NULL) { + ESP_LOGE(TAG, "No OTA partition found"); + s_status = OTA_STATUS_ERROR; + httpd_resp_send_err(req, HTTPD_500_INTERNAL_SERVER_ERROR, "No OTA partition"); + return ESP_FAIL; + } + + ESP_LOGI(TAG, "Writing to partition: %s", update_partition->label); + + /* Begin OTA */ + err = esp_ota_begin(update_partition, OTA_SIZE_UNKNOWN, &ota_handle); + if (err != ESP_OK) { + ESP_LOGE(TAG, "esp_ota_begin failed: %s", esp_err_to_name(err)); + s_status = OTA_STATUS_ERROR; + httpd_resp_send_err(req, HTTPD_500_INTERNAL_SERVER_ERROR, "OTA begin failed"); + return ESP_FAIL; + } + + /* Receive and write data */ + char buf[1024]; + int received; + + while (s_downloaded < s_total) { + received = httpd_req_recv(req, buf, sizeof(buf)); + if (received <= 0) { + if (received == HTTPD_SOCK_ERR_TIMEOUT) { + continue; + } + esp_ota_abort(ota_handle); + s_status = OTA_STATUS_ERROR; + httpd_resp_send_err(req, HTTPD_500_INTERNAL_SERVER_ERROR, "Receive error"); + return ESP_FAIL; + } + + err = esp_ota_write(ota_handle, buf, received); + if (err != ESP_OK) { + esp_ota_abort(ota_handle); + ESP_LOGE(TAG, "esp_ota_write failed: %s", esp_err_to_name(err)); + s_status = OTA_STATUS_ERROR; + httpd_resp_send_err(req, HTTPD_500_INTERNAL_SERVER_ERROR, "Write failed"); + return ESP_FAIL; + } + + s_downloaded += received; + + if (s_progress_cb) { + s_progress_cb(s_downloaded, s_total); + } + } + + s_status = OTA_STATUS_VERIFYING; + ESP_LOGI(TAG, "Download complete, verifying..."); + + /* End OTA */ + err = esp_ota_end(ota_handle); + if (err != ESP_OK) { + ESP_LOGE(TAG, "esp_ota_end failed: %s", esp_err_to_name(err)); + s_status = OTA_STATUS_ERROR; + httpd_resp_send_err(req, HTTPD_500_INTERNAL_SERVER_ERROR, "Verification failed"); + if (s_complete_cb) s_complete_cb(0); + return ESP_FAIL; + } + + /* Set boot partition */ + err = esp_ota_set_boot_partition(update_partition); + if (err != ESP_OK) { + ESP_LOGE(TAG, "esp_ota_set_boot_partition failed: %s", esp_err_to_name(err)); + s_status = OTA_STATUS_ERROR; + httpd_resp_send_err(req, HTTPD_500_INTERNAL_SERVER_ERROR, "Set boot failed"); + if (s_complete_cb) s_complete_cb(0); + return ESP_FAIL; + } + + s_status = OTA_STATUS_COMPLETE; + ESP_LOGI(TAG, "OTA complete! Rebooting..."); + + httpd_resp_sendstr(req, "OK"); + + if (s_complete_cb) s_complete_cb(1); + + /* Reboot after short delay */ + vTaskDelay(pdMS_TO_TICKS(1000)); + esp_restart(); + + return ESP_OK; +} + +/******************************************************************************* + * Public Functions + ******************************************************************************/ + +int8_t otaInit(void) +{ + s_status = OTA_STATUS_IDLE; + s_downloaded = 0; + s_total = 0; + + ESP_LOGI(TAG, "OTA system initialized"); + return 0; +} + +int8_t otaStartServer(void) +{ + if (s_server != NULL) { + ESP_LOGW(TAG, "Server already running"); + return 0; + } + + httpd_config_t config = HTTPD_DEFAULT_CONFIG(); + config.stack_size = 8192; + config.max_uri_handlers = 8; + + esp_err_t err = httpd_start(&s_server, &config); + if (err != ESP_OK) { + ESP_LOGE(TAG, "Failed to start server: %s", esp_err_to_name(err)); + return -1; + } + + /* Register handlers */ + httpd_uri_t page_uri = { + .uri = "/update", + .method = HTTP_GET, + .handler = otaPageHandler, + }; + httpd_register_uri_handler(s_server, &page_uri); + + httpd_uri_t upload_uri = { + .uri = "/update", + .method = HTTP_POST, + .handler = otaUploadHandler, + }; + httpd_register_uri_handler(s_server, &upload_uri); + + ESP_LOGI(TAG, "OTA server started on port %d", config.server_port); + return 0; +} + +int8_t otaStopServer(void) +{ + if (s_server == NULL) return 0; + + httpd_stop(s_server); + s_server = NULL; + + ESP_LOGI(TAG, "OTA server stopped"); + return 0; +} + +int8_t otaStartFromUrl(const char *url) +{ + /* TODO: Implement HTTP client OTA download */ + ESP_LOGW(TAG, "URL OTA not implemented yet: %s", url); + return -1; +} + +ota_status_t otaGetStatus(void) +{ + return s_status; +} + +void otaGetProgress(uint32_t *downloaded, uint32_t *total) +{ + if (downloaded) *downloaded = s_downloaded; + if (total) *total = s_total; +} + +void otaSetProgressCallback(ota_progress_cb_t callback) +{ + s_progress_cb = callback; +} + +void otaSetCompleteCallback(ota_complete_cb_t callback) +{ + s_complete_cb = callback; +} + +int8_t otaRollback(void) +{ + esp_err_t err = esp_ota_mark_app_invalid_rollback_and_reboot(); + if (err != ESP_OK) { + ESP_LOGE(TAG, "Rollback failed: %s", esp_err_to_name(err)); + return -1; + } + return 0; +} + +int8_t otaMarkValid(void) +{ + esp_err_t err = esp_ota_mark_app_valid_cancel_rollback(); + if (err != ESP_OK) { + ESP_LOGE(TAG, "Mark valid failed: %s", esp_err_to_name(err)); + return -1; + } + ESP_LOGI(TAG, "Firmware marked as valid"); + return 0; +} diff --git a/esp32 c3 Super Mini/main/ota_update.h b/esp32 c3 Super Mini/main/ota_update.h new file mode 100644 index 0000000..2a43b56 --- /dev/null +++ b/esp32 c3 Super Mini/main/ota_update.h @@ -0,0 +1,108 @@ +/** + * @file ota_update.h + * @brief OTA (Over-The-Air) Firmware Update for ESP32 + */ + +#ifndef OTA_UPDATE_H +#define OTA_UPDATE_H + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/******************************************************************************* + * OTA Status + ******************************************************************************/ +typedef enum { + OTA_STATUS_IDLE = 0, + OTA_STATUS_DOWNLOADING, + OTA_STATUS_VERIFYING, + OTA_STATUS_COMPLETE, + OTA_STATUS_ERROR, +} ota_status_t; + +/******************************************************************************* + * OTA Progress Callback + ******************************************************************************/ +typedef void (*ota_progress_cb_t)(uint32_t downloaded, uint32_t total); +typedef void (*ota_complete_cb_t)(int8_t success); + +/******************************************************************************* + * Function Prototypes + ******************************************************************************/ + +/** + * @brief Initialize OTA update system + * @return 0 on success, negative on error + */ +int8_t otaInit(void); + +/** + * @brief Start HTTP server for OTA upload + * @return 0 on success, negative on error + * + * @note Starts a web server on port 80 with upload page at /update + */ +int8_t otaStartServer(void); + +/** + * @brief Stop OTA server + * @return 0 on success, negative on error + */ +int8_t otaStopServer(void); + +/** + * @brief Start OTA from URL + * @param url HTTP(S) URL to firmware binary + * @return 0 on success, negative on error + */ +int8_t otaStartFromUrl(const char *url); + +/** + * @brief Get current OTA status + * @return OTA status + */ +ota_status_t otaGetStatus(void); + +/** + * @brief Get OTA progress + * @param downloaded Pointer to store downloaded bytes + * @param total Pointer to store total bytes + */ +void otaGetProgress(uint32_t *downloaded, uint32_t *total); + +/** + * @brief Set progress callback + * @param callback Function to call on progress + */ +void otaSetProgressCallback(ota_progress_cb_t callback); + +/** + * @brief Set completion callback + * @param callback Function to call on completion + */ +void otaSetCompleteCallback(ota_complete_cb_t callback); + +/** + * @brief Rollback to previous firmware + * @return 0 on success, negative on error + * + * @note Requires app rollback support in partition table + */ +int8_t otaRollback(void); + +/** + * @brief Mark current firmware as valid + * @return 0 on success, negative on error + * + * @note Should be called after successful boot + */ +int8_t otaMarkValid(void); + +#ifdef __cplusplus +} +#endif + +#endif /* OTA_UPDATE_H */ diff --git a/esp32 c3 Super Mini/main/sbus_receiver.c b/esp32 c3 Super Mini/main/sbus_receiver.c new file mode 100644 index 0000000..489aac1 --- /dev/null +++ b/esp32 c3 Super Mini/main/sbus_receiver.c @@ -0,0 +1,377 @@ +/** + * @file sbus_receiver.c + * @brief SBUS RC Receiver Implementation + */ + +#include "sbus_receiver.h" +#include "driver/uart.h" +#include "driver/gpio.h" +#include "esp_log.h" +#include "esp_timer.h" +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "freertos/queue.h" +#include + +static const char *TAG = "sbus"; + +/******************************************************************************* + * Private Variables + ******************************************************************************/ +static sbus_config_t s_config; +static sbus_frame_t s_frame; +static uint8_t s_is_initialized = 0; + +static sbus_frame_cb_t s_frame_cb = NULL; +static sbus_failsafe_cb_t s_failsafe_cb = NULL; + +static TaskHandle_t s_task_handle = NULL; +static QueueHandle_t s_uart_queue = NULL; + +/******************************************************************************* + * Private Functions + ******************************************************************************/ + +static uint32_t getTimeMs(void) +{ + return (uint32_t)(esp_timer_get_time() / 1000); +} + +/** + * @brief Parse SBUS frame + * @param buf 25-byte SBUS frame + * @param frame Parsed output + * @return 1 if valid, 0 if invalid + */ +static uint8_t parseSbusFrame(const uint8_t *buf, sbus_frame_t *frame) +{ + /* Validate header and footer */ + if (buf[0] != SBUS_FRAME_HEADER) { + return 0; + } + + /* + * SBUS uses 11-bit channels packed into bytes 1-22 + * Channel 0: bits 0-10 of bytes 1-2 + * Channel 1: bits 11-21 → bits 3-13 of bytes 2-3 + * ... and so on + * + * The packing is: + * buf[1-22] contains 16 channels × 11 bits = 176 bits = 22 bytes + */ + + frame->channels[0] = ((uint16_t)buf[1] | (uint16_t)buf[2] << 8) & 0x07FF; + frame->channels[1] = ((uint16_t)buf[2] >> 3 | (uint16_t)buf[3] << 5) & 0x07FF; + frame->channels[2] = ((uint16_t)buf[3] >> 6 | (uint16_t)buf[4] << 2 | (uint16_t)buf[5] << 10) & 0x07FF; + frame->channels[3] = ((uint16_t)buf[5] >> 1 | (uint16_t)buf[6] << 7) & 0x07FF; + frame->channels[4] = ((uint16_t)buf[6] >> 4 | (uint16_t)buf[7] << 4) & 0x07FF; + frame->channels[5] = ((uint16_t)buf[7] >> 7 | (uint16_t)buf[8] << 1 | (uint16_t)buf[9] << 9) & 0x07FF; + frame->channels[6] = ((uint16_t)buf[9] >> 2 | (uint16_t)buf[10] << 6) & 0x07FF; + frame->channels[7] = ((uint16_t)buf[10] >> 5 | (uint16_t)buf[11] << 3) & 0x07FF; + frame->channels[8] = ((uint16_t)buf[12] | (uint16_t)buf[13] << 8) & 0x07FF; + frame->channels[9] = ((uint16_t)buf[13] >> 3 | (uint16_t)buf[14] << 5) & 0x07FF; + frame->channels[10] = ((uint16_t)buf[14] >> 6 | (uint16_t)buf[15] << 2 | (uint16_t)buf[16] << 10) & 0x07FF; + frame->channels[11] = ((uint16_t)buf[16] >> 1 | (uint16_t)buf[17] << 7) & 0x07FF; + frame->channels[12] = ((uint16_t)buf[17] >> 4 | (uint16_t)buf[18] << 4) & 0x07FF; + frame->channels[13] = ((uint16_t)buf[18] >> 7 | (uint16_t)buf[19] << 1 | (uint16_t)buf[20] << 9) & 0x07FF; + frame->channels[14] = ((uint16_t)buf[20] >> 2 | (uint16_t)buf[21] << 6) & 0x07FF; + frame->channels[15] = ((uint16_t)buf[21] >> 5 | (uint16_t)buf[22] << 3) & 0x07FF; + + /* Parse flags byte (byte 23) */ + frame->flags.ch17 = (buf[23] & 0x01) ? 1 : 0; + frame->flags.ch18 = (buf[23] & 0x02) ? 1 : 0; + frame->flags.frame_lost = (buf[23] & 0x04) ? 1 : 0; + frame->flags.failsafe = (buf[23] & 0x08) ? 1 : 0; + + frame->timestamp_ms = getTimeMs(); + frame->is_valid = !frame->flags.failsafe && !frame->flags.frame_lost; + + return 1; +} + +/** + * @brief SBUS receive task + */ +static void sbusTask(void *arg) +{ + uint8_t buf[SBUS_FRAME_SIZE]; + int index = 0; + uint8_t byte; + + ESP_LOGI(TAG, "SBUS task started"); + + while (1) { + /* Read one byte at a time */ + int len = uart_read_bytes(s_config.uart_num, &byte, 1, pdMS_TO_TICKS(10)); + + if (len > 0) { + if (index == 0 && byte != SBUS_FRAME_HEADER) { + /* Wait for header */ + continue; + } + + buf[index++] = byte; + + if (index == SBUS_FRAME_SIZE) { + /* Complete frame received */ + if (parseSbusFrame(buf, &s_frame)) { + /* Callback */ + if (s_frame_cb) { + s_frame_cb(&s_frame); + } + + /* Check failsafe */ + if (s_frame.flags.failsafe && s_failsafe_cb) { + s_failsafe_cb(); + } + } + index = 0; + } + } else { + /* Timeout - check for failsafe */ + if (s_frame.is_valid) { + uint32_t elapsed = getTimeMs() - s_frame.timestamp_ms; + if (elapsed > SBUS_FAILSAFE_TIMEOUT_MS) { + s_frame.is_valid = 0; + s_frame.flags.failsafe = 1; + ESP_LOGW(TAG, "Signal lost (timeout)"); + if (s_failsafe_cb) { + s_failsafe_cb(); + } + } + } + } + } +} + +/******************************************************************************* + * Initialization + ******************************************************************************/ + +void sbusGetDefaultConfig(sbus_config_t *config) +{ + if (config == NULL) return; + + config->gpio_rx = 20; /* Adjust for your board */ + config->uart_num = 1; /* UART1 */ + config->inverted = 1; /* SBUS is inverted */ + + /* Default channel mapping (Betaflight/TAER) */ + config->roll_channel = 0; /* CH1 */ + config->pitch_channel = 1; /* CH2 */ + config->throttle_channel = 2; /* CH3 */ + config->yaw_channel = 3; /* CH4 */ + config->arm_channel = 4; /* CH5 */ + config->mode_channel = 5; /* CH6 */ +} + +int8_t sbusInit(const sbus_config_t *config) +{ + if (config == NULL) { + ESP_LOGE(TAG, "Invalid config"); + return -1; + } + + if (s_is_initialized) { + ESP_LOGW(TAG, "Already initialized"); + return 0; + } + + s_config = *config; + memset(&s_frame, 0, sizeof(s_frame)); + + /* Configure UART */ + uart_config_t uart_config = { + .baud_rate = SBUS_BAUD_RATE, + .data_bits = UART_DATA_8_BITS, + .parity = UART_PARITY_EVEN, + .stop_bits = UART_STOP_BITS_2, + .flow_ctrl = UART_HW_FLOWCTRL_DISABLE, + .source_clk = UART_SCLK_DEFAULT, + }; + + esp_err_t ret = uart_driver_install(config->uart_num, 256, 0, 10, &s_uart_queue, 0); + if (ret != ESP_OK) { + ESP_LOGE(TAG, "UART install failed: %s", esp_err_to_name(ret)); + return -2; + } + + ret = uart_param_config(config->uart_num, &uart_config); + if (ret != ESP_OK) { + ESP_LOGE(TAG, "UART config failed: %s", esp_err_to_name(ret)); + return -3; + } + + /* Set pins */ + ret = uart_set_pin(config->uart_num, UART_PIN_NO_CHANGE, config->gpio_rx, + UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE); + if (ret != ESP_OK) { + ESP_LOGE(TAG, "UART set pin failed: %s", esp_err_to_name(ret)); + return -4; + } + + /* Enable signal inversion if needed */ + if (config->inverted) { + uart_set_line_inverse(config->uart_num, UART_SIGNAL_RXD_INV); + } + + /* Create receiver task */ + BaseType_t task_ret = xTaskCreate(sbusTask, "sbus_rx", 4096, NULL, 10, &s_task_handle); + if (task_ret != pdPASS) { + ESP_LOGE(TAG, "Task create failed"); + return -5; + } + + s_is_initialized = 1; + ESP_LOGI(TAG, "SBUS initialized on GPIO %d (UART%d)", config->gpio_rx, config->uart_num); + + return 0; +} + +int8_t sbusDeinit(void) +{ + if (!s_is_initialized) return 0; + + if (s_task_handle) { + vTaskDelete(s_task_handle); + s_task_handle = NULL; + } + + uart_driver_delete(s_config.uart_num); + + s_is_initialized = 0; + ESP_LOGI(TAG, "SBUS deinitialized"); + + return 0; +} + +/******************************************************************************* + * Data Reading + ******************************************************************************/ + +int8_t sbusGetFrame(sbus_frame_t *frame) +{ + if (frame == NULL) return -1; + if (!s_is_initialized) return -2; + + *frame = s_frame; + return 0; +} + +int8_t sbusGetNormalized(sbus_normalized_t *data) +{ + if (data == NULL) return -1; + if (!s_is_initialized) return -2; + + sbus_frame_t frame = s_frame; + + /* Map channels to normalized values */ + data->roll = sbusNormalizeValue(frame.channels[s_config.roll_channel]); + data->pitch = sbusNormalizeValue(frame.channels[s_config.pitch_channel]); + data->yaw = sbusNormalizeValue(frame.channels[s_config.yaw_channel]); + data->throttle = sbusNormalizeThrottle(frame.channels[s_config.throttle_channel]); + + /* Aux channels */ + data->aux1 = sbusNormalizeValue(frame.channels[4]); + data->aux2 = sbusNormalizeValue(frame.channels[5]); + data->aux3 = sbusNormalizeValue(frame.channels[6]); + data->aux4 = sbusNormalizeValue(frame.channels[7]); + + /* Switches */ + data->arm_switch = sbusGetSwitchPosition(frame.channels[s_config.arm_channel]); + data->mode_switch = sbusGetSwitchPosition(frame.channels[s_config.mode_channel]); + data->alt_hold_switch = sbusGetSwitchPosition(frame.channels[6]); + + data->failsafe = frame.flags.failsafe || !frame.is_valid; + + return 0; +} + +int8_t sbusGetChannel(uint8_t channel, uint16_t *value) +{ + if (value == NULL) return -1; + if (!s_is_initialized) return -2; + if (channel >= SBUS_NUM_CHANNELS) return -3; + + *value = s_frame.channels[channel]; + return 0; +} + +/******************************************************************************* + * Status + ******************************************************************************/ + +uint8_t sbusIsValid(void) +{ + if (!s_is_initialized) return 0; + + /* Check timeout */ + uint32_t elapsed = getTimeMs() - s_frame.timestamp_ms; + if (elapsed > SBUS_FAILSAFE_TIMEOUT_MS) { + return 0; + } + + return s_frame.is_valid; +} + +uint8_t sbusIsFailsafe(void) +{ + if (!s_is_initialized) return 1; + return s_frame.flags.failsafe || !sbusIsValid(); +} + +uint32_t sbusGetTimeSinceLastFrame(void) +{ + if (!s_is_initialized) return 0xFFFFFFFF; + return getTimeMs() - s_frame.timestamp_ms; +} + +/******************************************************************************* + * Callbacks + ******************************************************************************/ + +void sbusSetFrameCallback(sbus_frame_cb_t callback) +{ + s_frame_cb = callback; +} + +void sbusSetFailsafeCallback(sbus_failsafe_cb_t callback) +{ + s_failsafe_cb = callback; +} + +/******************************************************************************* + * Utilities + ******************************************************************************/ + +float sbusNormalizeValue(uint16_t raw) +{ + /* Convert 172-1811 to -1.0 to +1.0 */ + float normalized = ((float)raw - SBUS_MID_VALUE) / ((float)(SBUS_MAX_VALUE - SBUS_MID_VALUE)); + + /* Clamp to -1 to +1 */ + if (normalized < -1.0f) normalized = -1.0f; + if (normalized > 1.0f) normalized = 1.0f; + + return normalized; +} + +float sbusNormalizeThrottle(uint16_t raw) +{ + /* Convert 172-1811 to 0.0 to 1.0 */ + float normalized = ((float)raw - SBUS_MIN_VALUE) / ((float)(SBUS_MAX_VALUE - SBUS_MIN_VALUE)); + + /* Clamp to 0 to 1 */ + if (normalized < 0.0f) normalized = 0.0f; + if (normalized > 1.0f) normalized = 1.0f; + + return normalized; +} + +int8_t sbusGetSwitchPosition(uint16_t raw) +{ + /* 3-position switch: low(-1), mid(0), high(+1) */ + if (raw < 500) return -1; + if (raw > 1500) return 1; + return 0; +} diff --git a/esp32 c3 Super Mini/main/sbus_receiver.h b/esp32 c3 Super Mini/main/sbus_receiver.h new file mode 100644 index 0000000..4a1317b --- /dev/null +++ b/esp32 c3 Super Mini/main/sbus_receiver.h @@ -0,0 +1,218 @@ +/** + * @file sbus_receiver.h + * @brief SBUS RC Receiver Driver + * @details Futaba SBUS protocol (inverted UART, 100kbaud) + * + * @note SBUS frame: 25 bytes, 8E2 format + * @note Frame period: ~7ms (analog) or ~9ms (highspeed) + */ + +#ifndef SBUS_RECEIVER_H +#define SBUS_RECEIVER_H + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/******************************************************************************* + * Configuration + ******************************************************************************/ +#define SBUS_NUM_CHANNELS 16 /* Standard SBUS channels */ +#define SBUS_FRAME_SIZE 25 /* Frame size in bytes */ +#define SBUS_BAUD_RATE 100000 /* 100kbaud */ +#define SBUS_FRAME_HEADER 0x0F /* Start byte */ +#define SBUS_FRAME_FOOTER 0x00 /* End byte */ + +/* Channel value range */ +#define SBUS_MIN_VALUE 172 /* Corresponds to ~1000µs */ +#define SBUS_MID_VALUE 992 /* Corresponds to ~1500µs */ +#define SBUS_MAX_VALUE 1811 /* Corresponds to ~2000µs */ + +/* Failsafe timeout */ +#define SBUS_FAILSAFE_TIMEOUT_MS 500 + +/******************************************************************************* + * SBUS Status Flags + ******************************************************************************/ +typedef struct { + uint8_t frame_lost : 1; /* Frame lost indicator */ + uint8_t failsafe : 1; /* Failsafe activated */ + uint8_t ch17 : 1; /* Digital channel 17 */ + uint8_t ch18 : 1; /* Digital channel 18 */ +} sbus_flags_t; + +/******************************************************************************* + * SBUS Frame Data + ******************************************************************************/ +typedef struct { + uint16_t channels[SBUS_NUM_CHANNELS]; /* Channel values (172-1811) */ + sbus_flags_t flags; + uint32_t timestamp_ms; /* Last update time */ + uint8_t is_valid; /* Data valid flag */ +} sbus_frame_t; + +/******************************************************************************* + * Normalized RC Data (for flight controller) + ******************************************************************************/ +typedef struct { + float roll; /* -1.0 to +1.0 */ + float pitch; /* -1.0 to +1.0 */ + float yaw; /* -1.0 to +1.0 */ + float throttle; /* 0.0 to 1.0 */ + + /* Aux channels */ + float aux1; /* CH5: -1.0 to +1.0 */ + float aux2; /* CH6: -1.0 to +1.0 */ + float aux3; /* CH7: -1.0 to +1.0 */ + float aux4; /* CH8: -1.0 to +1.0 */ + + /* Switches (3-position) */ + int8_t arm_switch; /* -1, 0, +1 */ + int8_t mode_switch; /* -1, 0, +1 */ + int8_t alt_hold_switch; /* -1, 0, +1 */ + + uint8_t failsafe; /* 1 = failsafe active */ +} sbus_normalized_t; + +/******************************************************************************* + * Configuration Structure + ******************************************************************************/ +typedef struct { + int gpio_rx; /* UART RX GPIO pin */ + uint8_t uart_num; /* UART port number (0, 1, or 2) */ + uint8_t inverted; /* 1 = signal is inverted (most SBUS) */ + + /* Channel mapping */ + uint8_t roll_channel; /* Default: 0 (CH1) */ + uint8_t pitch_channel; /* Default: 1 (CH2) */ + uint8_t throttle_channel; /* Default: 2 (CH3) */ + uint8_t yaw_channel; /* Default: 3 (CH4) */ + uint8_t arm_channel; /* Default: 4 (CH5) */ + uint8_t mode_channel; /* Default: 5 (CH6) */ +} sbus_config_t; + +/******************************************************************************* + * Callback Types + ******************************************************************************/ +typedef void (*sbus_frame_cb_t)(const sbus_frame_t *frame); +typedef void (*sbus_failsafe_cb_t)(void); + +/******************************************************************************* + * Function Prototypes - Initialization + ******************************************************************************/ + +/** + * @brief Get default SBUS configuration + * @param config Pointer to store default config + */ +void sbusGetDefaultConfig(sbus_config_t *config); + +/** + * @brief Initialize SBUS receiver + * @param config Pointer to configuration + * @return 0 on success, negative on error + */ +int8_t sbusInit(const sbus_config_t *config); + +/** + * @brief Deinitialize SBUS receiver + * @return 0 on success, negative on error + */ +int8_t sbusDeinit(void); + +/******************************************************************************* + * Function Prototypes - Data Reading + ******************************************************************************/ + +/** + * @brief Get latest SBUS frame data + * @param frame Pointer to store frame data + * @return 0 on success, negative on error + */ +int8_t sbusGetFrame(sbus_frame_t *frame); + +/** + * @brief Get normalized RC data + * @param data Pointer to store normalized data + * @return 0 on success, negative on error + */ +int8_t sbusGetNormalized(sbus_normalized_t *data); + +/** + * @brief Get single channel value (raw) + * @param channel Channel number (0-15) + * @param value Pointer to store value + * @return 0 on success, negative on error + */ +int8_t sbusGetChannel(uint8_t channel, uint16_t *value); + +/******************************************************************************* + * Function Prototypes - Status + ******************************************************************************/ + +/** + * @brief Check if signal is valid + * @return 1 if valid, 0 if failsafe/lost + */ +uint8_t sbusIsValid(void); + +/** + * @brief Check if failsafe is active + * @return 1 if failsafe, 0 otherwise + */ +uint8_t sbusIsFailsafe(void); + +/** + * @brief Get time since last valid frame + * @return Time in milliseconds + */ +uint32_t sbusGetTimeSinceLastFrame(void); + +/******************************************************************************* + * Function Prototypes - Callbacks + ******************************************************************************/ + +/** + * @brief Set frame received callback + * @param callback Function to call on new frame + */ +void sbusSetFrameCallback(sbus_frame_cb_t callback); + +/** + * @brief Set failsafe callback + * @param callback Function to call on failsafe + */ +void sbusSetFailsafeCallback(sbus_failsafe_cb_t callback); + +/******************************************************************************* + * Function Prototypes - Utilities + ******************************************************************************/ + +/** + * @brief Convert raw SBUS value to normalized (-1 to +1) + * @param raw Raw SBUS value (172-1811) + * @return Normalized value (-1.0 to +1.0) + */ +float sbusNormalizeValue(uint16_t raw); + +/** + * @brief Convert raw SBUS value to throttle (0 to 1) + * @param raw Raw SBUS value (172-1811) + * @return Throttle value (0.0 to 1.0) + */ +float sbusNormalizeThrottle(uint16_t raw); + +/** + * @brief Convert raw SBUS value to switch position + * @param raw Raw SBUS value + * @return -1, 0, or +1 (3-position switch) + */ +int8_t sbusGetSwitchPosition(uint16_t raw); + +#ifdef __cplusplus +} +#endif + +#endif /* SBUS_RECEIVER_H */ diff --git a/esp32 c3 Super Mini/main/web_server.c b/esp32 c3 Super Mini/main/web_server.c new file mode 100644 index 0000000..c057c41 --- /dev/null +++ b/esp32 c3 Super Mini/main/web_server.c @@ -0,0 +1,368 @@ +/** + * @file web_server.c + * @brief Embedded Web Server Implementation + */ + +#include "web_server.h" +#include "esp_http_server.h" +#include "esp_log.h" +#include "cJSON.h" +#include + +static const char *TAG = "web_srv"; + +/******************************************************************************* + * Private Variables + ******************************************************************************/ +static httpd_handle_t s_server = NULL; +static uint8_t s_is_initialized = 0; + +static web_arm_cb_t s_arm_cb = NULL; +static web_pid_cb_t s_pid_cb = NULL; +static web_get_telemetry_cb_t s_telemetry_cb = NULL; +static web_get_pid_cb_t s_get_pid_cb = NULL; + +/******************************************************************************* + * Dashboard HTML + ******************************************************************************/ +static const char *DASHBOARD_HTML = +"" +"" +"Quad FC Dashboard" +"" +"" +"

🚁 Flight Controller Dashboard

" +"
" +"
" +"

📐 Attitude

" +"
" +"
0.0
Roll °
" +"
0.0
Pitch °
" +"
0.0
Yaw °
" +"
" +"

🔧 Motors

" +"
" +"
M1
" +"
M2
" +"
M3
" +"
M4
" +"
" +"

🎮 Control

" +"
Disconnected
" +"
" +"" +"
" +"

⚙️ Rate PID (Roll/Pitch)

" +"
" +"
P
" +"
I
" +"
D
" +"
" +"" +"
" +"
" +""; + +/******************************************************************************* + * HTTP Handlers + ******************************************************************************/ + +/* GET / - Serve dashboard */ +static esp_err_t dashboardHandler(httpd_req_t *req) +{ + httpd_resp_set_type(req, "text/html"); + httpd_resp_send(req, DASHBOARD_HTML, strlen(DASHBOARD_HTML)); + return ESP_OK; +} + +/* GET /api/telemetry - Get telemetry JSON */ +static esp_err_t telemetryHandler(httpd_req_t *req) +{ + web_telemetry_t tel = {0}; + + if (s_telemetry_cb) { + s_telemetry_cb(&tel); + } + + cJSON *root = cJSON_CreateObject(); + cJSON_AddNumberToObject(root, "roll", tel.roll); + cJSON_AddNumberToObject(root, "pitch", tel.pitch); + cJSON_AddNumberToObject(root, "yaw", tel.yaw); + cJSON_AddNumberToObject(root, "throttle", tel.throttle); + + cJSON *motors = cJSON_CreateArray(); + for (int i = 0; i < 4; i++) { + cJSON_AddItemToArray(motors, cJSON_CreateNumber(tel.motor[i])); + } + cJSON_AddItemToObject(root, "motor", motors); + + cJSON_AddBoolToObject(root, "armed", tel.armed); + cJSON_AddNumberToObject(root, "mode", tel.mode); + cJSON_AddNumberToObject(root, "battery", tel.battery_voltage); + + char *json = cJSON_PrintUnformatted(root); + httpd_resp_set_type(req, "application/json"); + httpd_resp_send(req, json, strlen(json)); + + free(json); + cJSON_Delete(root); + + return ESP_OK; +} + +/* POST /api/arm - Arm/disarm */ +static esp_err_t armHandler(httpd_req_t *req) +{ + char buf[64]; + int len = httpd_req_recv(req, buf, sizeof(buf) - 1); + if (len <= 0) { + httpd_resp_send_err(req, HTTPD_400_BAD_REQUEST, "No data"); + return ESP_FAIL; + } + buf[len] = '\0'; + + cJSON *root = cJSON_Parse(buf); + if (root == NULL) { + httpd_resp_send_err(req, HTTPD_400_BAD_REQUEST, "Invalid JSON"); + return ESP_FAIL; + } + + cJSON *arm_json = cJSON_GetObjectItem(root, "arm"); + uint8_t arm = cJSON_IsTrue(arm_json) ? 1 : 0; + + if (s_arm_cb) { + s_arm_cb(arm); + } + + cJSON_Delete(root); + + /* Response */ + cJSON *resp = cJSON_CreateObject(); + cJSON_AddBoolToObject(resp, "armed", arm); + cJSON_AddStringToObject(resp, "status", "ok"); + + char *json = cJSON_PrintUnformatted(resp); + httpd_resp_set_type(req, "application/json"); + httpd_resp_send(req, json, strlen(json)); + + free(json); + cJSON_Delete(resp); + + ESP_LOGI(TAG, "Arm command: %d", arm); + + return ESP_OK; +} + +/* POST /api/pid - Set PID gains */ +static esp_err_t pidHandler(httpd_req_t *req) +{ + char buf[256]; + int len = httpd_req_recv(req, buf, sizeof(buf) - 1); + if (len <= 0) { + httpd_resp_send_err(req, HTTPD_400_BAD_REQUEST, "No data"); + return ESP_FAIL; + } + buf[len] = '\0'; + + cJSON *root = cJSON_Parse(buf); + if (root == NULL) { + httpd_resp_send_err(req, HTTPD_400_BAD_REQUEST, "Invalid JSON"); + return ESP_FAIL; + } + + web_pid_gains_t gains = {0}; + + cJSON *item; + if ((item = cJSON_GetObjectItem(root, "rate_rp_p"))) gains.rate_rp_p = (float)item->valuedouble; + if ((item = cJSON_GetObjectItem(root, "rate_rp_i"))) gains.rate_rp_i = (float)item->valuedouble; + if ((item = cJSON_GetObjectItem(root, "rate_rp_d"))) gains.rate_rp_d = (float)item->valuedouble; + if ((item = cJSON_GetObjectItem(root, "rate_yaw_p"))) gains.rate_yaw_p = (float)item->valuedouble; + if ((item = cJSON_GetObjectItem(root, "rate_yaw_i"))) gains.rate_yaw_i = (float)item->valuedouble; + if ((item = cJSON_GetObjectItem(root, "rate_yaw_d"))) gains.rate_yaw_d = (float)item->valuedouble; + if ((item = cJSON_GetObjectItem(root, "att_rp_p"))) gains.att_rp_p = (float)item->valuedouble; + if ((item = cJSON_GetObjectItem(root, "att_rp_i"))) gains.att_rp_i = (float)item->valuedouble; + if ((item = cJSON_GetObjectItem(root, "att_rp_d"))) gains.att_rp_d = (float)item->valuedouble; + + if (s_pid_cb) { + s_pid_cb(&gains); + } + + cJSON_Delete(root); + + httpd_resp_set_type(req, "application/json"); + httpd_resp_sendstr(req, "{\"status\":\"ok\"}"); + + ESP_LOGI(TAG, "PID updated: P=%.3f I=%.3f D=%.4f", + gains.rate_rp_p, gains.rate_rp_i, gains.rate_rp_d); + + return ESP_OK; +} + +/* GET /api/pid - Get PID gains */ +static esp_err_t getPidHandler(httpd_req_t *req) +{ + web_pid_gains_t gains = {0}; + + if (s_get_pid_cb) { + s_get_pid_cb(&gains); + } + + cJSON *root = cJSON_CreateObject(); + cJSON_AddNumberToObject(root, "rate_rp_p", gains.rate_rp_p); + cJSON_AddNumberToObject(root, "rate_rp_i", gains.rate_rp_i); + cJSON_AddNumberToObject(root, "rate_rp_d", gains.rate_rp_d); + cJSON_AddNumberToObject(root, "rate_yaw_p", gains.rate_yaw_p); + cJSON_AddNumberToObject(root, "rate_yaw_i", gains.rate_yaw_i); + cJSON_AddNumberToObject(root, "rate_yaw_d", gains.rate_yaw_d); + cJSON_AddNumberToObject(root, "att_rp_p", gains.att_rp_p); + cJSON_AddNumberToObject(root, "att_rp_i", gains.att_rp_i); + cJSON_AddNumberToObject(root, "att_rp_d", gains.att_rp_d); + + char *json = cJSON_PrintUnformatted(root); + httpd_resp_set_type(req, "application/json"); + httpd_resp_send(req, json, strlen(json)); + + free(json); + cJSON_Delete(root); + + return ESP_OK; +} + +/******************************************************************************* + * Public Functions + ******************************************************************************/ + +int8_t webServerInit(void) +{ + s_is_initialized = 1; + ESP_LOGI(TAG, "Web server initialized"); + return 0; +} + +int8_t webServerStart(void) +{ + if (s_server != NULL) { + ESP_LOGW(TAG, "Server already running"); + return 0; + } + + httpd_config_t config = HTTPD_DEFAULT_CONFIG(); + config.stack_size = 8192; + config.max_uri_handlers = 10; + + esp_err_t ret = httpd_start(&s_server, &config); + if (ret != ESP_OK) { + ESP_LOGE(TAG, "Failed to start server: %s", esp_err_to_name(ret)); + return -1; + } + + /* Register handlers */ + httpd_uri_t dashboard = { .uri = "/", .method = HTTP_GET, .handler = dashboardHandler }; + httpd_uri_t telemetry = { .uri = "/api/telemetry", .method = HTTP_GET, .handler = telemetryHandler }; + httpd_uri_t arm = { .uri = "/api/arm", .method = HTTP_POST, .handler = armHandler }; + httpd_uri_t pid_set = { .uri = "/api/pid", .method = HTTP_POST, .handler = pidHandler }; + httpd_uri_t pid_get = { .uri = "/api/pid", .method = HTTP_GET, .handler = getPidHandler }; + + httpd_register_uri_handler(s_server, &dashboard); + httpd_register_uri_handler(s_server, &telemetry); + httpd_register_uri_handler(s_server, &arm); + httpd_register_uri_handler(s_server, &pid_set); + httpd_register_uri_handler(s_server, &pid_get); + + ESP_LOGI(TAG, "Web server started on port %d", config.server_port); + return 0; +} + +int8_t webServerStop(void) +{ + if (s_server == NULL) return 0; + + httpd_stop(s_server); + s_server = NULL; + + ESP_LOGI(TAG, "Web server stopped"); + return 0; +} + +void webServerSetArmCallback(web_arm_cb_t callback) +{ + s_arm_cb = callback; +} + +void webServerSetPidCallback(web_pid_cb_t callback) +{ + s_pid_cb = callback; +} + +void webServerSetTelemetryCallback(web_get_telemetry_cb_t callback) +{ + s_telemetry_cb = callback; +} + +void webServerSetGetPidCallback(web_get_pid_cb_t callback) +{ + s_get_pid_cb = callback; +} diff --git a/esp32 c3 Super Mini/main/web_server.h b/esp32 c3 Super Mini/main/web_server.h new file mode 100644 index 0000000..8bf933f --- /dev/null +++ b/esp32 c3 Super Mini/main/web_server.h @@ -0,0 +1,104 @@ +/** + * @file web_server.h + * @brief Embedded Web Server for Flight Controller Dashboard + */ + +#ifndef WEB_SERVER_H +#define WEB_SERVER_H + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/******************************************************************************* + * Configuration + ******************************************************************************/ +#define WEB_SERVER_PORT 80 +#define WEB_MAX_CONNECTIONS 4 + +/******************************************************************************* + * Telemetry Data Structure (for API) + ******************************************************************************/ +typedef struct { + float roll; + float pitch; + float yaw; + float throttle; + float motor[4]; + float gyro[3]; + float accel[3]; + float battery_voltage; + uint8_t armed; + uint8_t mode; +} web_telemetry_t; + +/******************************************************************************* + * PID Gains Structure + ******************************************************************************/ +typedef struct { + float rate_rp_p, rate_rp_i, rate_rp_d; + float rate_yaw_p, rate_yaw_i, rate_yaw_d; + float att_rp_p, att_rp_i, att_rp_d; +} web_pid_gains_t; + +/******************************************************************************* + * Callback Types + ******************************************************************************/ +typedef void (*web_arm_cb_t)(uint8_t arm); +typedef void (*web_pid_cb_t)(const web_pid_gains_t *gains); +typedef void (*web_get_telemetry_cb_t)(web_telemetry_t *telemetry); +typedef void (*web_get_pid_cb_t)(web_pid_gains_t *gains); + +/******************************************************************************* + * Function Prototypes + ******************************************************************************/ + +/** + * @brief Initialize web server + * @return 0 on success, negative on error + */ +int8_t webServerInit(void); + +/** + * @brief Start web server + * @return 0 on success, negative on error + */ +int8_t webServerStart(void); + +/** + * @brief Stop web server + * @return 0 on success, negative on error + */ +int8_t webServerStop(void); + +/** + * @brief Set arm/disarm callback + * @param callback Function to call on arm command + */ +void webServerSetArmCallback(web_arm_cb_t callback); + +/** + * @brief Set PID update callback + * @param callback Function to call on PID update + */ +void webServerSetPidCallback(web_pid_cb_t callback); + +/** + * @brief Set telemetry getter callback + * @param callback Function to call to get telemetry + */ +void webServerSetTelemetryCallback(web_get_telemetry_cb_t callback); + +/** + * @brief Set PID getter callback + * @param callback Function to call to get PID gains + */ +void webServerSetGetPidCallback(web_get_pid_cb_t callback); + +#ifdef __cplusplus +} +#endif + +#endif /* WEB_SERVER_H */ diff --git a/esp32 c3 Super Mini/main/wifi_manager.c b/esp32 c3 Super Mini/main/wifi_manager.c new file mode 100644 index 0000000..7aa831f --- /dev/null +++ b/esp32 c3 Super Mini/main/wifi_manager.c @@ -0,0 +1,425 @@ +/** + * @file wifi_manager.c + * @brief WiFi Manager Implementation + */ + +#include "wifi_manager.h" +#include "esp_wifi.h" +#include "esp_event.h" +#include "esp_log.h" +#include "esp_netif.h" +#include "nvs_flash.h" +#include "nvs.h" +#include "freertos/FreeRTOS.h" +#include "freertos/event_groups.h" +#include + +static const char *TAG = "wifi_mgr"; + +/******************************************************************************* + * NVS Keys + ******************************************************************************/ +#define NVS_NAMESPACE "wifi_creds" +#define NVS_KEY_SSID "ssid" +#define NVS_KEY_PASS "password" + +/******************************************************************************* + * Event Group Bits + ******************************************************************************/ +#define WIFI_CONNECTED_BIT BIT0 +#define WIFI_FAIL_BIT BIT1 + +/******************************************************************************* + * Private Variables + ******************************************************************************/ +static EventGroupHandle_t s_wifi_event_group = NULL; +static esp_netif_t *s_sta_netif = NULL; +static esp_netif_t *s_ap_netif = NULL; + +static wifi_manager_mode_t s_current_mode = WIFI_MODE_DISABLED; +static wifi_manager_status_t s_current_status = WIFI_STATUS_DISCONNECTED; +static char s_ip_addr[16] = {0}; + +static wifi_connected_cb_t s_connected_cb = NULL; +static wifi_disconnected_cb_t s_disconnected_cb = NULL; + +static uint8_t s_is_initialized = 0; +static int s_retry_count = 0; +#define MAX_RETRY 5 + +/******************************************************************************* + * Event Handler + ******************************************************************************/ +static void wifiEventHandler(void *arg, esp_event_base_t event_base, + int32_t event_id, void *event_data) +{ + if (event_base == WIFI_EVENT) { + switch (event_id) { + case WIFI_EVENT_STA_START: + ESP_LOGI(TAG, "STA started, connecting..."); + s_current_status = WIFI_STATUS_CONNECTING; + esp_wifi_connect(); + break; + + case WIFI_EVENT_STA_DISCONNECTED: + if (s_retry_count < MAX_RETRY) { + esp_wifi_connect(); + s_retry_count++; + ESP_LOGI(TAG, "Retry connecting (%d/%d)", s_retry_count, MAX_RETRY); + } else { + xEventGroupSetBits(s_wifi_event_group, WIFI_FAIL_BIT); + s_current_status = WIFI_STATUS_CONNECTION_FAILED; + ESP_LOGW(TAG, "Connection failed after %d retries", MAX_RETRY); + if (s_disconnected_cb) s_disconnected_cb(); + } + break; + + case WIFI_EVENT_AP_START: + ESP_LOGI(TAG, "AP started"); + s_current_status = WIFI_STATUS_AP_STARTED; + break; + + case WIFI_EVENT_AP_STACONNECTED: + { + wifi_event_ap_staconnected_t *event = (wifi_event_ap_staconnected_t *)event_data; + ESP_LOGI(TAG, "Station connected: " MACSTR, MAC2STR(event->mac)); + break; + } + + case WIFI_EVENT_AP_STADISCONNECTED: + { + wifi_event_ap_stadisconnected_t *event = (wifi_event_ap_stadisconnected_t *)event_data; + ESP_LOGI(TAG, "Station disconnected: " MACSTR, MAC2STR(event->mac)); + break; + } + + default: + break; + } + } else if (event_base == IP_EVENT) { + if (event_id == IP_EVENT_STA_GOT_IP) { + ip_event_got_ip_t *event = (ip_event_got_ip_t *)event_data; + snprintf(s_ip_addr, sizeof(s_ip_addr), IPSTR, IP2STR(&event->ip_info.ip)); + ESP_LOGI(TAG, "Got IP: %s", s_ip_addr); + s_retry_count = 0; + s_current_status = WIFI_STATUS_CONNECTED; + xEventGroupSetBits(s_wifi_event_group, WIFI_CONNECTED_BIT); + if (s_connected_cb) s_connected_cb(); + } + } +} + +/******************************************************************************* + * Initialization + ******************************************************************************/ + +int8_t wifiManagerInit(void) +{ + if (s_is_initialized) { + ESP_LOGW(TAG, "Already initialized"); + return 0; + } + + esp_err_t ret; + + /* Initialize NVS */ + ret = nvs_flash_init(); + if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) { + ESP_ERROR_CHECK(nvs_flash_erase()); + ret = nvs_flash_init(); + } + ESP_ERROR_CHECK(ret); + + /* Initialize TCP/IP stack */ + ESP_ERROR_CHECK(esp_netif_init()); + + /* Create default event loop */ + ESP_ERROR_CHECK(esp_event_loop_create_default()); + + /* Create event group */ + s_wifi_event_group = xEventGroupCreate(); + if (s_wifi_event_group == NULL) { + ESP_LOGE(TAG, "Failed to create event group"); + return -1; + } + + /* Create default network interfaces */ + s_sta_netif = esp_netif_create_default_wifi_sta(); + s_ap_netif = esp_netif_create_default_wifi_ap(); + + /* Initialize WiFi with default config */ + wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT(); + ESP_ERROR_CHECK(esp_wifi_init(&cfg)); + + /* Register event handlers */ + ESP_ERROR_CHECK(esp_event_handler_instance_register( + WIFI_EVENT, ESP_EVENT_ANY_ID, &wifiEventHandler, NULL, NULL)); + ESP_ERROR_CHECK(esp_event_handler_instance_register( + IP_EVENT, IP_EVENT_STA_GOT_IP, &wifiEventHandler, NULL, NULL)); + + s_is_initialized = 1; + ESP_LOGI(TAG, "WiFi Manager initialized"); + + return 0; +} + +int8_t wifiManagerDeinit(void) +{ + if (!s_is_initialized) return 0; + + esp_wifi_stop(); + esp_wifi_deinit(); + + if (s_sta_netif) esp_netif_destroy(s_sta_netif); + if (s_ap_netif) esp_netif_destroy(s_ap_netif); + + if (s_wifi_event_group) { + vEventGroupDelete(s_wifi_event_group); + s_wifi_event_group = NULL; + } + + s_is_initialized = 0; + return 0; +} + +/******************************************************************************* + * AP Mode + ******************************************************************************/ + +int8_t wifiManagerStartAP(const char *ssid, const char *password) +{ + if (!s_is_initialized) { + ESP_LOGE(TAG, "Not initialized"); + return -1; + } + + const char *ap_ssid = (ssid != NULL) ? ssid : WIFI_AP_SSID_DEFAULT; + const char *ap_pass = (password != NULL) ? password : WIFI_AP_PASS_DEFAULT; + + wifi_config_t wifi_config = { + .ap = { + .channel = WIFI_AP_CHANNEL, + .max_connection = WIFI_AP_MAX_CONN, + .authmode = WIFI_AUTH_WPA2_PSK, + .pmf_cfg = { + .required = false, + }, + }, + }; + + strncpy((char *)wifi_config.ap.ssid, ap_ssid, sizeof(wifi_config.ap.ssid) - 1); + wifi_config.ap.ssid_len = strlen(ap_ssid); + strncpy((char *)wifi_config.ap.password, ap_pass, sizeof(wifi_config.ap.password) - 1); + + if (strlen(ap_pass) < 8) { + wifi_config.ap.authmode = WIFI_AUTH_OPEN; + } + + ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_AP)); + ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_AP, &wifi_config)); + ESP_ERROR_CHECK(esp_wifi_start()); + + s_current_mode = WIFI_MODE_AP; + + /* Get AP IP address */ + esp_netif_ip_info_t ip_info; + esp_netif_get_ip_info(s_ap_netif, &ip_info); + snprintf(s_ip_addr, sizeof(s_ip_addr), IPSTR, IP2STR(&ip_info.ip)); + + ESP_LOGI(TAG, "AP started: SSID=%s, IP=%s", ap_ssid, s_ip_addr); + + return 0; +} + +int8_t wifiManagerStopAP(void) +{ + if (!s_is_initialized) return -1; + + esp_wifi_stop(); + s_current_mode = WIFI_MODE_DISABLED; + s_current_status = WIFI_STATUS_DISCONNECTED; + + return 0; +} + +/******************************************************************************* + * STA Mode + ******************************************************************************/ + +int8_t wifiManagerConnect(const char *ssid, const char *password) +{ + if (!s_is_initialized) { + ESP_LOGE(TAG, "Not initialized"); + return -1; + } + + if (ssid == NULL || strlen(ssid) == 0) { + ESP_LOGE(TAG, "Invalid SSID"); + return -2; + } + + wifi_config_t wifi_config = { + .sta = { + .threshold.authmode = WIFI_AUTH_WPA2_PSK, + }, + }; + + strncpy((char *)wifi_config.sta.ssid, ssid, sizeof(wifi_config.sta.ssid) - 1); + if (password != NULL) { + strncpy((char *)wifi_config.sta.password, password, sizeof(wifi_config.sta.password) - 1); + } + + s_retry_count = 0; + + ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA)); + ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_STA, &wifi_config)); + ESP_ERROR_CHECK(esp_wifi_start()); + + s_current_mode = WIFI_MODE_STA; + + ESP_LOGI(TAG, "Connecting to SSID: %s", ssid); + + /* Wait for connection */ + EventBits_t bits = xEventGroupWaitBits(s_wifi_event_group, + WIFI_CONNECTED_BIT | WIFI_FAIL_BIT, + pdTRUE, pdFALSE, pdMS_TO_TICKS(15000)); + + if (bits & WIFI_CONNECTED_BIT) { + return 0; + } else if (bits & WIFI_FAIL_BIT) { + return -3; + } + + return -4; /* Timeout */ +} + +int8_t wifiManagerDisconnect(void) +{ + if (!s_is_initialized) return -1; + + esp_wifi_disconnect(); + s_current_status = WIFI_STATUS_DISCONNECTED; + + return 0; +} + +/******************************************************************************* + * Credentials Storage + ******************************************************************************/ + +int8_t wifiManagerSaveCredentials(const wifi_credentials_t *creds) +{ + if (creds == NULL) return -1; + + nvs_handle_t handle; + esp_err_t err = nvs_open(NVS_NAMESPACE, NVS_READWRITE, &handle); + if (err != ESP_OK) { + ESP_LOGE(TAG, "NVS open failed: %s", esp_err_to_name(err)); + return -2; + } + + err = nvs_set_str(handle, NVS_KEY_SSID, creds->ssid); + if (err != ESP_OK) { + nvs_close(handle); + return -3; + } + + err = nvs_set_str(handle, NVS_KEY_PASS, creds->password); + if (err != ESP_OK) { + nvs_close(handle); + return -4; + } + + err = nvs_commit(handle); + nvs_close(handle); + + ESP_LOGI(TAG, "Credentials saved"); + return (err == ESP_OK) ? 0 : -5; +} + +int8_t wifiManagerLoadCredentials(wifi_credentials_t *creds) +{ + if (creds == NULL) return -1; + + nvs_handle_t handle; + esp_err_t err = nvs_open(NVS_NAMESPACE, NVS_READONLY, &handle); + if (err != ESP_OK) { + return -2; /* No saved credentials */ + } + + size_t ssid_len = sizeof(creds->ssid); + size_t pass_len = sizeof(creds->password); + + err = nvs_get_str(handle, NVS_KEY_SSID, creds->ssid, &ssid_len); + if (err != ESP_OK) { + nvs_close(handle); + return -3; + } + + err = nvs_get_str(handle, NVS_KEY_PASS, creds->password, &pass_len); + if (err != ESP_OK) { + nvs_close(handle); + return -4; + } + + nvs_close(handle); + ESP_LOGI(TAG, "Credentials loaded: SSID=%s", creds->ssid); + return 0; +} + +int8_t wifiManagerClearCredentials(void) +{ + nvs_handle_t handle; + esp_err_t err = nvs_open(NVS_NAMESPACE, NVS_READWRITE, &handle); + if (err != ESP_OK) return -1; + + nvs_erase_all(handle); + nvs_commit(handle); + nvs_close(handle); + + ESP_LOGI(TAG, "Credentials cleared"); + return 0; +} + +/******************************************************************************* + * Status + ******************************************************************************/ + +int8_t wifiManagerGetInfo(wifi_connection_info_t *info) +{ + if (info == NULL) return -1; + + info->mode = s_current_mode; + info->status = s_current_status; + strncpy(info->ip_addr, s_ip_addr, sizeof(info->ip_addr)); + + if (s_current_status == WIFI_STATUS_CONNECTED) { + wifi_ap_record_t ap_info; + if (esp_wifi_sta_get_ap_info(&ap_info) == ESP_OK) { + info->rssi = ap_info.rssi; + } + } else { + info->rssi = 0; + } + + return 0; +} + +uint8_t wifiManagerIsConnected(void) +{ + return (s_current_status == WIFI_STATUS_CONNECTED) ? 1 : 0; +} + +/******************************************************************************* + * Callbacks + ******************************************************************************/ + +void wifiManagerSetConnectedCallback(wifi_connected_cb_t callback) +{ + s_connected_cb = callback; +} + +void wifiManagerSetDisconnectedCallback(wifi_disconnected_cb_t callback) +{ + s_disconnected_cb = callback; +} diff --git a/esp32 c3 Super Mini/main/wifi_manager.h b/esp32 c3 Super Mini/main/wifi_manager.h new file mode 100644 index 0000000..431272c --- /dev/null +++ b/esp32 c3 Super Mini/main/wifi_manager.h @@ -0,0 +1,186 @@ +/** + * @file wifi_manager.h + * @brief WiFi Manager for ESP32 Flight Controller + * @details Supports AP mode (configuration) and STA mode (home network) + */ + +#ifndef WIFI_MANAGER_H +#define WIFI_MANAGER_H + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/******************************************************************************* + * Configuration + ******************************************************************************/ +#define WIFI_SSID_MAX_LEN 32 +#define WIFI_PASS_MAX_LEN 64 +#define WIFI_AP_SSID_DEFAULT "QUAD-FC-SETUP" +#define WIFI_AP_PASS_DEFAULT "12345678" +#define WIFI_AP_CHANNEL 6 +#define WIFI_AP_MAX_CONN 4 + +/******************************************************************************* + * WiFi Mode + ******************************************************************************/ +typedef enum { + WIFI_MODE_DISABLED = 0, + WIFI_MODE_AP, /* Access Point mode (configuration) */ + WIFI_MODE_STA, /* Station mode (connect to network) */ + WIFI_MODE_APSTA, /* Both AP and STA mode */ +} wifi_manager_mode_t; + +/******************************************************************************* + * Connection Status + ******************************************************************************/ +typedef enum { + WIFI_STATUS_DISCONNECTED = 0, + WIFI_STATUS_CONNECTING, + WIFI_STATUS_CONNECTED, + WIFI_STATUS_CONNECTION_FAILED, + WIFI_STATUS_AP_STARTED, +} wifi_manager_status_t; + +/******************************************************************************* + * WiFi Credentials + ******************************************************************************/ +typedef struct { + char ssid[WIFI_SSID_MAX_LEN]; + char password[WIFI_PASS_MAX_LEN]; +} wifi_credentials_t; + +/******************************************************************************* + * Connection Info + ******************************************************************************/ +typedef struct { + wifi_manager_mode_t mode; + wifi_manager_status_t status; + char ip_addr[16]; /* e.g., "192.168.4.1" */ + char gateway[16]; + char netmask[16]; + int8_t rssi; /* Signal strength (STA mode) */ +} wifi_connection_info_t; + +/******************************************************************************* + * Callback Types + ******************************************************************************/ +typedef void (*wifi_connected_cb_t)(void); +typedef void (*wifi_disconnected_cb_t)(void); + +/******************************************************************************* + * Function Prototypes - Initialization + ******************************************************************************/ + +/** + * @brief Initialize WiFi manager + * @return 0 on success, negative on error + */ +int8_t wifiManagerInit(void); + +/** + * @brief Deinitialize WiFi manager + * @return 0 on success, negative on error + */ +int8_t wifiManagerDeinit(void); + +/******************************************************************************* + * Function Prototypes - AP Mode + ******************************************************************************/ + +/** + * @brief Start Access Point mode + * @param ssid AP network name (NULL for default) + * @param password AP password (NULL for default, min 8 chars) + * @return 0 on success, negative on error + */ +int8_t wifiManagerStartAP(const char *ssid, const char *password); + +/** + * @brief Stop Access Point mode + * @return 0 on success, negative on error + */ +int8_t wifiManagerStopAP(void); + +/******************************************************************************* + * Function Prototypes - STA Mode + ******************************************************************************/ + +/** + * @brief Connect to WiFi network + * @param ssid Network SSID + * @param password Network password + * @return 0 on success, negative on error + */ +int8_t wifiManagerConnect(const char *ssid, const char *password); + +/** + * @brief Disconnect from current network + * @return 0 on success, negative on error + */ +int8_t wifiManagerDisconnect(void); + +/******************************************************************************* + * Function Prototypes - Credentials Storage (NVS) + ******************************************************************************/ + +/** + * @brief Save WiFi credentials to NVS + * @param creds Pointer to credentials + * @return 0 on success, negative on error + */ +int8_t wifiManagerSaveCredentials(const wifi_credentials_t *creds); + +/** + * @brief Load WiFi credentials from NVS + * @param creds Pointer to store credentials + * @return 0 on success, negative on error (no saved credentials) + */ +int8_t wifiManagerLoadCredentials(wifi_credentials_t *creds); + +/** + * @brief Clear saved credentials from NVS + * @return 0 on success, negative on error + */ +int8_t wifiManagerClearCredentials(void); + +/******************************************************************************* + * Function Prototypes - Status + ******************************************************************************/ + +/** + * @brief Get current connection info + * @param info Pointer to store connection info + * @return 0 on success, negative on error + */ +int8_t wifiManagerGetInfo(wifi_connection_info_t *info); + +/** + * @brief Check if connected + * @return 1 if connected, 0 otherwise + */ +uint8_t wifiManagerIsConnected(void); + +/******************************************************************************* + * Function Prototypes - Callbacks + ******************************************************************************/ + +/** + * @brief Set connection callback + * @param callback Function to call on connection + */ +void wifiManagerSetConnectedCallback(wifi_connected_cb_t callback); + +/** + * @brief Set disconnection callback + * @param callback Function to call on disconnection + */ +void wifiManagerSetDisconnectedCallback(wifi_disconnected_cb_t callback); + +#ifdef __cplusplus +} +#endif + +#endif /* WIFI_MANAGER_H */