sl-firmware
fa75c442a7
Archive STM32 firmware to legacy/stm32/: - src/, include/, lib/USB_CDC/, platformio.ini, test stubs, flash_firmware.py - test/test_battery_adc.c, test_hw_button.c, test_pid_schedule.c, test_vesc_can.c, test_can_watchdog.c - USB_CDC_BUG.md Rename: stm32_protocol → esp32_protocol, mamba_protocol → balance_protocol, stm32_cmd_node → esp32_cmd_node, stm32_cmd_params → esp32_cmd_params, stm32_cmd.launch.py → esp32_cmd.launch.py, test_stm32_protocol → test_esp32_protocol, test_stm32_cmd_node → test_esp32_cmd_node Content cleanup across all files: - Mamba F722S → ESP32-S3 BALANCE - BlackPill → ESP32-S3 IO - STM32F722/F7xx → ESP32-S3 - stm32Mode/Version/Port → esp32Mode/Version/Port - STM32 State/Mode labels → ESP32 State/Mode - Jetson Nano → Jetson Orin Nano Super - /dev/stm32 → /dev/esp32 - stm32_bridge → esp32_bridge - STM32 HAL → ESP-IDF docs/SALTYLAB.md: - Update "Drone FC Details" to describe ESP32-S3 BALANCE board (Waveshare ESP32-S3 Touch LCD 1.28) - Replace verbose "Self-Balancing Control" STM32 section with brief note pointing to SAUL-TEE-SYSTEM-REFERENCE.md TEAM.md: Update Embedded Firmware Engineer role to ESP32-S3 / ESP-IDF No new functionality — cleanup only. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
101 lines
3.5 KiB
C
101 lines
3.5 KiB
C
/* imu_cal_flash.c — IMU mount angle calibration flash storage (Issue #680)
|
|
*
|
|
* Stores pitch/roll mount offsets in STM32F722 flash sector 7 at 0x0807FF00.
|
|
* Preserves existing PID records (pid_sched_flash_t + pid_flash_t) across
|
|
* the mandatory sector erase by reading them into RAM before erasing.
|
|
*/
|
|
|
|
#include "imu_cal_flash.h"
|
|
#include "pid_flash.h"
|
|
#include "stm32f7xx_hal.h"
|
|
#include <string.h>
|
|
|
|
bool imu_cal_flash_load(float *pitch_offset, float *roll_offset)
|
|
{
|
|
const imu_cal_flash_t *p = (const imu_cal_flash_t *)IMU_CAL_FLASH_ADDR;
|
|
|
|
if (p->magic != IMU_CAL_FLASH_MAGIC) return false;
|
|
|
|
/* Sanity-check: mount offsets beyond ±90° indicate a corrupt record */
|
|
if (p->pitch_offset < -90.0f || p->pitch_offset > 90.0f) return false;
|
|
if (p->roll_offset < -90.0f || p->roll_offset > 90.0f) return false;
|
|
|
|
*pitch_offset = p->pitch_offset;
|
|
*roll_offset = p->roll_offset;
|
|
return true;
|
|
}
|
|
|
|
/* Write 'len' bytes (multiple of 4) from 'src' to flash at 'addr'.
|
|
* Flash must be unlocked by caller. */
|
|
static HAL_StatusTypeDef write_words(uint32_t addr,
|
|
const void *src,
|
|
uint32_t len)
|
|
{
|
|
const uint32_t *p = (const uint32_t *)src;
|
|
for (uint32_t i = 0; i < len / 4u; i++) {
|
|
HAL_StatusTypeDef rc = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD,
|
|
addr, p[i]);
|
|
if (rc != HAL_OK) return rc;
|
|
addr += 4u;
|
|
}
|
|
return HAL_OK;
|
|
}
|
|
|
|
bool imu_cal_flash_save(float pitch_offset, float roll_offset)
|
|
{
|
|
/* Snapshot PID records BEFORE erasing so we can restore them */
|
|
pid_flash_t pid_snap;
|
|
pid_sched_flash_t sched_snap;
|
|
memcpy(&pid_snap, (const void *)PID_FLASH_STORE_ADDR, sizeof(pid_snap));
|
|
memcpy(&sched_snap, (const void *)PID_SCHED_FLASH_ADDR, sizeof(sched_snap));
|
|
|
|
HAL_StatusTypeDef rc;
|
|
|
|
rc = HAL_FLASH_Unlock();
|
|
if (rc != HAL_OK) return false;
|
|
|
|
/* Erase sector 7 (covers all three records) */
|
|
FLASH_EraseInitTypeDef erase = {
|
|
.TypeErase = FLASH_TYPEERASE_SECTORS,
|
|
.Sector = PID_FLASH_SECTOR,
|
|
.NbSectors = 1,
|
|
.VoltageRange = PID_FLASH_SECTOR_VOLTAGE,
|
|
};
|
|
uint32_t sector_error = 0;
|
|
rc = HAL_FLASHEx_Erase(&erase, §or_error);
|
|
if (rc != HAL_OK || sector_error != 0xFFFFFFFFUL) {
|
|
HAL_FLASH_Lock();
|
|
return false;
|
|
}
|
|
|
|
/* Write new IMU calibration record at 0x0807FF00 */
|
|
imu_cal_flash_t cal;
|
|
memset(&cal, 0xFF, sizeof(cal));
|
|
cal.magic = IMU_CAL_FLASH_MAGIC;
|
|
cal.pitch_offset = pitch_offset;
|
|
cal.roll_offset = roll_offset;
|
|
|
|
rc = write_words(IMU_CAL_FLASH_ADDR, &cal, sizeof(cal));
|
|
if (rc != HAL_OK) { HAL_FLASH_Lock(); return false; }
|
|
|
|
/* Restore PID gain schedule if it was valid */
|
|
if (sched_snap.magic == PID_SCHED_MAGIC) {
|
|
rc = write_words(PID_SCHED_FLASH_ADDR, &sched_snap, sizeof(sched_snap));
|
|
if (rc != HAL_OK) { HAL_FLASH_Lock(); return false; }
|
|
}
|
|
|
|
/* Restore single-PID record if it was valid */
|
|
if (pid_snap.magic == PID_FLASH_MAGIC) {
|
|
rc = write_words(PID_FLASH_STORE_ADDR, &pid_snap, sizeof(pid_snap));
|
|
if (rc != HAL_OK) { HAL_FLASH_Lock(); return false; }
|
|
}
|
|
|
|
HAL_FLASH_Lock();
|
|
|
|
/* Verify readback */
|
|
const imu_cal_flash_t *v = (const imu_cal_flash_t *)IMU_CAL_FLASH_ADDR;
|
|
return (v->magic == IMU_CAL_FLASH_MAGIC &&
|
|
v->pitch_offset == pitch_offset &&
|
|
v->roll_offset == roll_offset);
|
|
}
|