sprint-7: Commissioning wizard + relay ZN auto-tuner + NVS config + knob encoder
Python: - autotuner.py: RelayAutoTuner (Astrom-Hagglund relay method) → TunerResult with Ku/Tu; TunerResult.to_pid_gains() applies ZN formulas - commissioning_wizard.py: 4-phase state machine (RUDDER_LIMITS → SENSOR_CAL → AUTO_TUNE → DONE); transport-injected for testability; abort on invalid cal - test_autotuner.py: 17 tests covering relay convergence, ZN formulas, wizard full-run, abort, ADC swap, identical-ADC guard Firmware: - nvs_config.h/cpp: NVS-backed CalibrationData store (adc limits, rudder angles, outer Kp/Ki/Kd, commissioned flag); float stored as uint32 via memcpy - knob_encoder.h/cpp: quadrature rotary encoder on GPIO 16/17 with ISR Gray-code decode; knob_arm coil arms for 5 s window; updates heading setpoint ±1 deg/detent - modbus_slave.cpp: COIL_CMD_KNOB_ARM now calls knob_encoder_set_armed() - main.cpp: nvs_config_init/load at boot; apply commissioned calibration to rudder sensor and outer loop gains; start knob encoder task Tests: 326 passed | Flash: 28.5% Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
@@ -0,0 +1,129 @@
|
||||
// =============================================================================
|
||||
// hal/knob_encoder.cpp -- Rotary encoder for heading knob (Sprint 7)
|
||||
// =============================================================================
|
||||
|
||||
#include "knob_encoder.h"
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
#include "../hal/pinout.h"
|
||||
#include "../modes/standby.h"
|
||||
#include "../pid/pid_outer_task.h"
|
||||
#include "../system/ar_log.h"
|
||||
#include "../system/task_config.h"
|
||||
|
||||
namespace arautopilot::hal {
|
||||
|
||||
namespace {
|
||||
|
||||
constexpr const char* TAG = "AR/KNOB";
|
||||
|
||||
// Encoder pins (reuse DO6/DO7 pads in commissioning variant).
|
||||
constexpr uint8_t PIN_A = PIN_DO6_RESERVED; // GPIO 16
|
||||
constexpr uint8_t PIN_B = PIN_DO7_RESERVED; // GPIO 17
|
||||
|
||||
// ----- Quadrature state machine (Gray-code decode) -------------------------
|
||||
//
|
||||
// State transitions: 4 states × 2 new pins → delta
|
||||
// Table index: (prev_state << 2) | new_ab
|
||||
// Value: +1 (CW), -1 (CCW), 0 (no move or invalid)
|
||||
//
|
||||
static const int8_t QEM[16] = {
|
||||
// AB=00 01 10 11
|
||||
0, -1, +1, 0, // prev=00
|
||||
+1, 0, 0, -1, // prev=01
|
||||
-1, 0, 0, +1, // prev=10
|
||||
0, +1, -1, 0, // prev=11
|
||||
};
|
||||
|
||||
volatile int32_t g_pulse_count = 0;
|
||||
volatile uint8_t g_prev_state = 0;
|
||||
|
||||
portMUX_TYPE g_mux = portMUX_INITIALIZER_UNLOCKED;
|
||||
|
||||
// Armed state
|
||||
volatile bool g_armed = false;
|
||||
volatile uint32_t g_arm_time_ms = 0;
|
||||
|
||||
void IRAM_ATTR encoder_isr() {
|
||||
uint8_t a = (uint8_t)digitalRead(PIN_A);
|
||||
uint8_t b = (uint8_t)digitalRead(PIN_B);
|
||||
uint8_t state = (a << 1) | b;
|
||||
int8_t delta = QEM[(g_prev_state << 2) | state];
|
||||
if (delta != 0) {
|
||||
portENTER_CRITICAL_ISR(&g_mux);
|
||||
g_pulse_count += delta;
|
||||
portEXIT_CRITICAL_ISR(&g_mux);
|
||||
}
|
||||
g_prev_state = state;
|
||||
}
|
||||
|
||||
void KnobTask(void* /*pv*/) {
|
||||
AR_LOGI(TAG, "knob_encoder task started on core %d", xPortGetCoreID());
|
||||
|
||||
int32_t last_count = 0;
|
||||
|
||||
for (;;) {
|
||||
const uint32_t now = millis();
|
||||
|
||||
// Auto-disarm after timeout.
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
bool armed = g_armed;
|
||||
if (armed && (now - g_arm_time_ms) > KNOB_ARM_TIMEOUT_MS) {
|
||||
g_armed = false;
|
||||
armed = false;
|
||||
AR_LOGI(TAG, "knob auto-disarmed (timeout)");
|
||||
}
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
int32_t count = g_pulse_count;
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
|
||||
int32_t delta = count - last_count;
|
||||
last_count = count;
|
||||
|
||||
if (delta != 0 && armed && !modes::is_standby()) {
|
||||
// Apply ±KNOB_STEP_DEG per pulse, normalised to 0..360.
|
||||
float sp = pid::pid_outer_heading_setpoint_deg();
|
||||
sp = fmodf(sp + (float)delta * KNOB_STEP_DEG + 360.0f, 360.0f);
|
||||
pid::pid_outer_set_heading_setpoint_deg(sp);
|
||||
AR_LOGD(TAG, "knob: delta=%d new_sp=%.1f", delta, sp);
|
||||
}
|
||||
|
||||
vTaskDelay(pdMS_TO_TICKS(20)); // 50 Hz check rate
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
void knob_encoder_init() {
|
||||
pinMode(PIN_A, INPUT_PULLUP);
|
||||
pinMode(PIN_B, INPUT_PULLUP);
|
||||
g_prev_state = (uint8_t)((digitalRead(PIN_A) << 1) | digitalRead(PIN_B));
|
||||
attachInterrupt(digitalPinToInterrupt(PIN_A), encoder_isr, CHANGE);
|
||||
attachInterrupt(digitalPinToInterrupt(PIN_B), encoder_isr, CHANGE);
|
||||
AR_LOGI(TAG, "knob encoder on GPIO A=%d B=%d", PIN_A, PIN_B);
|
||||
}
|
||||
|
||||
void knob_encoder_start_task() {
|
||||
xTaskCreatePinnedToCore(KnobTask, "knob_enc",
|
||||
2048, nullptr,
|
||||
1, // low priority
|
||||
nullptr,
|
||||
AR_TASK_CORE_COMMS);
|
||||
}
|
||||
|
||||
void knob_encoder_set_armed(bool armed) {
|
||||
portENTER_CRITICAL(&g_mux);
|
||||
g_armed = armed;
|
||||
g_arm_time_ms = millis();
|
||||
portEXIT_CRITICAL(&g_mux);
|
||||
AR_LOGI(TAG, "knob %s", armed ? "ARMED" : "disarmed");
|
||||
}
|
||||
|
||||
bool knob_encoder_is_armed() {
|
||||
return g_armed;
|
||||
}
|
||||
|
||||
} // namespace arautopilot::hal
|
||||
@@ -0,0 +1,49 @@
|
||||
// =============================================================================
|
||||
// hal/knob_encoder.h -- Rotary encoder for heading knob (Sprint 7)
|
||||
// =============================================================================
|
||||
//
|
||||
// A quadrature rotary encoder connected to GPIO pins drives heading setpoint
|
||||
// changes. Clockwise rotation increases the setpoint by STEP_DEG per detent;
|
||||
// counter-clockwise decreases it.
|
||||
//
|
||||
// The encoder is only active when the pilot is engaged (any mode other than
|
||||
// STANDBY). While disengaged, pulses are silently discarded.
|
||||
//
|
||||
// The "knob arm" coil (COIL_CMD_KNOB_ARM via Modbus, addr 3) enables the
|
||||
// knob for one KNOB_ARM_TIMEOUT_MS window. If no edge arrives within that
|
||||
// window, the coil auto-clears. This prevents unintended heading changes
|
||||
// from vibration noise.
|
||||
//
|
||||
// GPIO assignment (not on AR-NMEA-IO v1.0 main header -- uses DO6/DO7 pads
|
||||
// repurposed as inputs in the commissioning firmware variant):
|
||||
// PIN_KNOB_A = PIN_DO6_RESERVED (GPIO 16)
|
||||
// PIN_KNOB_B = PIN_DO7_RESERVED (GPIO 17)
|
||||
//
|
||||
// The interrupt handler uses a 4-state Gray-code decode table for noise
|
||||
// immunity.
|
||||
// =============================================================================
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
namespace arautopilot::hal {
|
||||
|
||||
constexpr float KNOB_STEP_DEG = 1.0f; ///< heading change per detent
|
||||
constexpr uint32_t KNOB_ARM_TIMEOUT_MS = 5000; ///< arm window before auto-disarm
|
||||
|
||||
/// Initialise encoder GPIO and attach interrupts.
|
||||
/// Must be called from setup() after do_init().
|
||||
void knob_encoder_init();
|
||||
|
||||
/// Start the knob processing task (low priority, Core 0).
|
||||
void knob_encoder_start_task();
|
||||
|
||||
/// Set the armed state. While armed the knob updates the heading setpoint.
|
||||
/// The coil write handler calls this.
|
||||
void knob_encoder_set_armed(bool armed);
|
||||
|
||||
/// Return current armed state.
|
||||
bool knob_encoder_is_armed();
|
||||
|
||||
} // namespace arautopilot::hal
|
||||
@@ -0,0 +1,121 @@
|
||||
// =============================================================================
|
||||
// hal/nvs_config.cpp -- NVS-backed calibration store (Sprint 7)
|
||||
// =============================================================================
|
||||
|
||||
#include "nvs_config.h"
|
||||
|
||||
#include <nvs.h>
|
||||
#include <nvs_flash.h>
|
||||
#include <Arduino.h>
|
||||
|
||||
#include "../system/ar_log.h"
|
||||
|
||||
namespace arautopilot::hal {
|
||||
|
||||
namespace {
|
||||
constexpr const char* TAG = "AR/NVS";
|
||||
constexpr const char* NS = "ar_cal";
|
||||
} // namespace
|
||||
|
||||
bool nvs_config_init() {
|
||||
esp_err_t err = nvs_flash_init();
|
||||
if (err == ESP_ERR_NVS_NO_FREE_PAGES ||
|
||||
err == ESP_ERR_NVS_NEW_VERSION_FOUND) {
|
||||
AR_LOGW(TAG, "NVS partition needs erase (err=0x%x) -- factory-resetting", err);
|
||||
nvs_flash_erase();
|
||||
err = nvs_flash_init();
|
||||
}
|
||||
if (err != ESP_OK) {
|
||||
AR_LOGE(TAG, "nvs_flash_init failed: 0x%x", err);
|
||||
return false;
|
||||
}
|
||||
AR_LOGI(TAG, "NVS initialised (namespace: %s)", NS);
|
||||
return true;
|
||||
}
|
||||
|
||||
CalibrationData nvs_config_load() {
|
||||
CalibrationData cal; // defaults in struct definition
|
||||
|
||||
nvs_handle_t h;
|
||||
if (nvs_open(NS, NVS_READONLY, &h) != ESP_OK) {
|
||||
AR_LOGI(TAG, "No calibration in NVS -- using factory defaults");
|
||||
return cal;
|
||||
}
|
||||
|
||||
// Helper lambdas: read or keep default on missing key.
|
||||
auto get_i16 = [&](const char* key, int16_t& out) {
|
||||
int16_t v;
|
||||
if (nvs_get_i16(h, key, &v) == ESP_OK) out = v;
|
||||
};
|
||||
auto get_float = [&](const char* key, float& out) {
|
||||
// NVS has no native float; store as uint32 via memcpy.
|
||||
uint32_t raw;
|
||||
if (nvs_get_u32(h, key, &raw) == ESP_OK) {
|
||||
memcpy(&out, &raw, sizeof(float));
|
||||
}
|
||||
};
|
||||
auto get_u8 = [&](const char* key, uint8_t& out) {
|
||||
uint8_t v;
|
||||
if (nvs_get_u8(h, key, &v) == ESP_OK) out = v;
|
||||
};
|
||||
|
||||
get_i16("adc_port", cal.adc_port);
|
||||
get_i16("adc_stbd", cal.adc_stbd);
|
||||
get_float("rud_port", cal.rudder_port_deg);
|
||||
get_float("rud_stbd", cal.rudder_stbd_deg);
|
||||
get_float("ou_kp", cal.outer_kp);
|
||||
get_float("ou_ki", cal.outer_ki);
|
||||
get_float("ou_kd", cal.outer_kd);
|
||||
uint8_t comm = 0;
|
||||
get_u8("commissioned", comm);
|
||||
cal.commissioned = (comm == 1);
|
||||
|
||||
nvs_close(h);
|
||||
AR_LOGI(TAG, "Calibration loaded (commissioned=%d adc=%d..%d)",
|
||||
(int)cal.commissioned, cal.adc_port, cal.adc_stbd);
|
||||
return cal;
|
||||
}
|
||||
|
||||
bool nvs_config_save(const CalibrationData& cal) {
|
||||
nvs_handle_t h;
|
||||
if (nvs_open(NS, NVS_READWRITE, &h) != ESP_OK) {
|
||||
AR_LOGE(TAG, "nvs_open for write failed");
|
||||
return false;
|
||||
}
|
||||
|
||||
auto put_float = [&](const char* key, float v) {
|
||||
uint32_t raw;
|
||||
memcpy(&raw, &v, sizeof(float));
|
||||
nvs_set_u32(h, key, raw);
|
||||
};
|
||||
|
||||
nvs_set_i16(h, "adc_port", cal.adc_port);
|
||||
nvs_set_i16(h, "adc_stbd", cal.adc_stbd);
|
||||
put_float("rud_port", cal.rudder_port_deg);
|
||||
put_float("rud_stbd", cal.rudder_stbd_deg);
|
||||
put_float("ou_kp", cal.outer_kp);
|
||||
put_float("ou_ki", cal.outer_ki);
|
||||
put_float("ou_kd", cal.outer_kd);
|
||||
nvs_set_u8(h, "commissioned", cal.commissioned ? 1u : 0u);
|
||||
|
||||
esp_err_t err = nvs_commit(h);
|
||||
nvs_close(h);
|
||||
if (err != ESP_OK) {
|
||||
AR_LOGE(TAG, "nvs_commit failed: 0x%x", err);
|
||||
return false;
|
||||
}
|
||||
AR_LOGI(TAG, "Calibration saved to NVS");
|
||||
return true;
|
||||
}
|
||||
|
||||
bool nvs_config_erase() {
|
||||
nvs_handle_t h;
|
||||
if (nvs_open(NS, NVS_READWRITE, &h) != ESP_OK) return false;
|
||||
esp_err_t err = nvs_erase_all(h);
|
||||
nvs_commit(h);
|
||||
nvs_close(h);
|
||||
AR_LOGW(TAG, "NVS namespace '%s' erased (factory reset)", NS);
|
||||
return err == ESP_OK;
|
||||
}
|
||||
|
||||
} // namespace arautopilot::hal
|
||||
@@ -0,0 +1,54 @@
|
||||
// =============================================================================
|
||||
// hal/nvs_config.h -- NVS-backed calibration store (Sprint 7)
|
||||
// =============================================================================
|
||||
//
|
||||
// Persists commissioning data to the ESP32 NVS (Non-Volatile Storage) under
|
||||
// the namespace "ar_cal".
|
||||
//
|
||||
// Keys written:
|
||||
// "adc_port" -- int16_t raw ADC at full-port rudder limit
|
||||
// "adc_stbd" -- int16_t raw ADC at full-stbd rudder limit
|
||||
// "rud_port" -- float physical angle at port limit (deg, default -35)
|
||||
// "rud_stbd" -- float physical angle at stbd limit (deg, default +35)
|
||||
// "ou_kp" -- float outer-loop recommended Kp from commissioning
|
||||
// "ou_ki" -- float outer-loop recommended Ki
|
||||
// "ou_kd" -- float outer-loop recommended Kd
|
||||
// "commissioned"-- uint8_t 1 if the unit has been commissioned
|
||||
//
|
||||
// All reads/writes are synchronous and should only be called at startup or
|
||||
// from a low-priority task (not the 50 Hz safety task).
|
||||
// =============================================================================
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
namespace arautopilot::hal {
|
||||
|
||||
struct CalibrationData {
|
||||
int16_t adc_port = 0; ///< raw ADC at port limit
|
||||
int16_t adc_stbd = 4095; ///< raw ADC at stbd limit
|
||||
float rudder_port_deg = -35.0f; ///< physical port angle
|
||||
float rudder_stbd_deg = 35.0f; ///< physical stbd angle
|
||||
float outer_kp = 1.0f; ///< recommended outer Kp
|
||||
float outer_ki = 0.0f; ///< recommended outer Ki
|
||||
float outer_kd = 0.0f; ///< recommended outer Kd
|
||||
bool commissioned = false; ///< true once commissioning ran
|
||||
};
|
||||
|
||||
/// Initialise the NVS namespace. Must be called from setup() before any
|
||||
/// other nvs_config function. Returns false if NVS flash is unrecoverable
|
||||
/// (factory reset required).
|
||||
bool nvs_config_init();
|
||||
|
||||
/// Read calibration data from NVS. Returns default values if the key is
|
||||
/// absent (first boot or factory-reset).
|
||||
CalibrationData nvs_config_load();
|
||||
|
||||
/// Write calibration data to NVS.
|
||||
bool nvs_config_save(const CalibrationData& cal);
|
||||
|
||||
/// Erase the entire "ar_cal" namespace (factory reset).
|
||||
bool nvs_config_erase();
|
||||
|
||||
} // namespace arautopilot::hal
|
||||
@@ -23,6 +23,8 @@
|
||||
#include <esp_system.h>
|
||||
|
||||
#include "hal/di_do.h"
|
||||
#include "hal/knob_encoder.h"
|
||||
#include "hal/nvs_config.h"
|
||||
#include "hal/pinout.h"
|
||||
#include "hal/rudder_actuator.h"
|
||||
#include "hal/rudder_sensor.h"
|
||||
@@ -66,11 +68,27 @@ void setup() {
|
||||
// Initialise the pilot mode state machine (boots in STANDBY).
|
||||
arautopilot::modes::mode_init();
|
||||
|
||||
// NVS (non-volatile config + calibration). Must be first HAL call.
|
||||
arautopilot::hal::nvs_config_init();
|
||||
const auto cal = arautopilot::hal::nvs_config_load();
|
||||
if (cal.commissioned) {
|
||||
AR_LOGI(TAG, "commissioned unit: adc %d..%d outer Kp=%.3f",
|
||||
cal.adc_port, cal.adc_stbd, cal.outer_kp);
|
||||
} else {
|
||||
AR_LOGW(TAG, "not commissioned -- using factory defaults");
|
||||
}
|
||||
|
||||
// Initialise hardware abstraction layer.
|
||||
arautopilot::hal::di_init();
|
||||
arautopilot::hal::do_init();
|
||||
arautopilot::hal::rudder_sensor_init();
|
||||
if (cal.commissioned) {
|
||||
arautopilot::hal::rudder_sensor_set_calibration(
|
||||
cal.adc_port, cal.adc_stbd,
|
||||
cal.rudder_port_deg, cal.rudder_stbd_deg);
|
||||
}
|
||||
arautopilot::hal::rudder_actuator_init();
|
||||
arautopilot::hal::knob_encoder_init();
|
||||
|
||||
// Safety: watchdog + DI override task. Initialised first so the
|
||||
// watchdog is armed before slower subsystems come up.
|
||||
@@ -86,15 +104,21 @@ void setup() {
|
||||
|
||||
arautopilot::safety::safety_monitor_start_task();
|
||||
arautopilot::hal::rudder_sensor_start_task();
|
||||
arautopilot::hal::knob_encoder_start_task();
|
||||
|
||||
// PID inner loop (Sprint 2). Active only when not in STANDBY and
|
||||
// rudder sensor reading is valid; refuses internally otherwise.
|
||||
arautopilot::pid::pid_inner_task_init();
|
||||
arautopilot::pid::pid_inner_task_start();
|
||||
|
||||
// PID outer loop (Sprint 3) -- Heading Hold. Only writes to the inner
|
||||
// loop's setpoint when current mode == HEADING_HOLD.
|
||||
// PID outer loop (Sprint 3) -- Heading Hold.
|
||||
arautopilot::pid::pid_outer_task_init();
|
||||
if (cal.commissioned && cal.outer_kp > 0.0f) {
|
||||
arautopilot::pid::pid_outer_update_gains(
|
||||
cal.outer_kp, cal.outer_ki, cal.outer_kd);
|
||||
AR_LOGI(TAG, "applied commissioned outer gains Kp=%.3f Ki=%.3f Kd=%.3f",
|
||||
cal.outer_kp, cal.outer_ki, cal.outer_kd);
|
||||
}
|
||||
arautopilot::pid::pid_outer_task_start();
|
||||
|
||||
ar_start_heartbeat_task();
|
||||
|
||||
@@ -33,6 +33,7 @@
|
||||
#include "../system/task_config.h"
|
||||
#include "modbus_registers.h"
|
||||
#include "nmea2000_consumer.h"
|
||||
#include "../hal/knob_encoder.h"
|
||||
#include "../safety/safety_monitor.h"
|
||||
|
||||
namespace arautopilot::protocols::modbus {
|
||||
@@ -461,6 +462,7 @@ Modbus::Error write_coil(uint16_t addr, bool value) {
|
||||
return Modbus::Error::SUCCESS;
|
||||
case COIL_CMD_KNOB_ARM:
|
||||
g_coils.knob_arm = value;
|
||||
hal::knob_encoder_set_armed(value);
|
||||
return Modbus::Error::SUCCESS;
|
||||
default:
|
||||
return Modbus::Error::ILLEGAL_DATA_ADDRESS;
|
||||
|
||||
Reference in New Issue
Block a user