sl-firmware
9ed678ca35
Issue #680 — IMU mount angle calibration: - imu_cal_flash.h/.c: store pitch/roll offsets in flash sector 7 (0x0807FF00, 64 bytes; preserves PID records across sector erase) - mpu6000_set_mount_offset(): subtracts offsets from pitch/roll output - mpu6000_has_mount_offset(): reports cal_status=2 to Orin - 'O' CDC command: capture current pitch/roll → save to flash → ACK JSON - Load offsets on boot; report in printf log CAN telemetry correction (Tee: production has no USB to Orin): - FC_IMU (0x402): pitch/roll/yaw/cal_status/balance_state at 50 Hz - orin_can_broadcast_imu() rate-limited to ORIN_IMU_TLM_HZ (50 Hz) - FC_BARO (0x403): pressure_pa/temp_x10/alt_cm at 1 Hz (Issue #672) - orin_can_broadcast_baro() rate-limited to ORIN_BARO_TLM_HZ (1 Hz) Issue #685 — LED CAN override: - ORIN_CAN_ID_LED_CMD (0x304): pattern/brightness/duration_ms from Orin - orin_can_led_override volatile state + orin_can_led_updated flag - main.c: apply pattern to LED state machine on each LED_CMD received Orin side: - saltybot_can_node.py: production SocketCAN bridge — reads 0x400-0x403, publishes /saltybot/imu, /saltybot/balance_state, /saltybot/barometer; subscribes /cmd_vel → 0x301 DRIVE; /saltybot/leds → 0x304 LED_CMD; sends 0x300 HEARTBEAT at 5 Hz; sends 0x303 ESTOP on shutdown - setup.py: register saltybot_can_node entry point + uart_bridge launch Fix: re-apply --defsym __stack_end=_estack-0x1000 linker fix to branch 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);
|
|
}
|