Merge branch 'claude/elated-sammet-86a4ab'
This commit is contained in:
@@ -0,0 +1,138 @@
|
||||
// =============================================================================
|
||||
// rudder_actuator_electric.h -- reversible DC motor actuator (Sprint 4)
|
||||
// =============================================================================
|
||||
//
|
||||
// Drives a reversible DC motor with mechanical end-stops (Lewmar, Simpson
|
||||
// Lawrence) via H-bridge (DRV8833 or equivalent):
|
||||
// DO1 (GPIO) -- IN1 (port direction)
|
||||
// DO2 (GPIO) -- IN2 (starboard direction)
|
||||
// DO3 (GPIO) -- master power relay (ENA)
|
||||
// PWM channel -- motor speed (shared LEDC channel 1)
|
||||
//
|
||||
// Architecture identical to HydraulicRudderActuator; only the PWM mapping
|
||||
// differs (electric motors typically need a minimum useful duty to overcome
|
||||
// static friction — see AR_ACTUATOR_DEADBAND_PCT in firmware_config.h).
|
||||
// =============================================================================
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
|
||||
#include <Preferences.h>
|
||||
#include <driver/ledc.h>
|
||||
|
||||
#include "hal/di_do.h"
|
||||
#include "hal/pinout.h"
|
||||
#include "hal/rudder_actuator_iface.h"
|
||||
#include "hal/rudder_sensor.h"
|
||||
#include "system/ar_log.h"
|
||||
|
||||
#ifndef AR_ACTUATOR_DEADBAND_PCT
|
||||
# define AR_ACTUATOR_DEADBAND_PCT 5.0f
|
||||
#endif
|
||||
|
||||
namespace arautopilot::hal {
|
||||
|
||||
class ElectricDcRudderActuator final : public IRudderActuator {
|
||||
public:
|
||||
static constexpr const char* NVS_NS = "ar_act";
|
||||
static constexpr const char* NVS_KEY = "angle_limit";
|
||||
static constexpr float DEFAULT_LIMIT_DEG = 35.0f;
|
||||
static constexpr float MIN_LIMIT_DEG = 15.0f;
|
||||
static constexpr float MAX_LIMIT_DEG = 45.0f;
|
||||
|
||||
void init() override {
|
||||
_prefs.begin(NVS_NS, false);
|
||||
_limit_deg = _prefs.getFloat(NVS_KEY, DEFAULT_LIMIT_DEG);
|
||||
_limit_deg = std::clamp(_limit_deg, MIN_LIMIT_DEG, MAX_LIMIT_DEG);
|
||||
_prefs.end();
|
||||
|
||||
pinMode(PIN_DO1, OUTPUT); digitalWrite(PIN_DO1, LOW);
|
||||
pinMode(PIN_DO2, OUTPUT); digitalWrite(PIN_DO2, LOW);
|
||||
pinMode(PIN_DO3, OUTPUT); digitalWrite(PIN_DO3, LOW);
|
||||
|
||||
ledc_timer_config_t timer{};
|
||||
timer.speed_mode = LEDC_LOW_SPEED_MODE;
|
||||
timer.duty_resolution = LEDC_TIMER_8_BIT;
|
||||
timer.timer_num = LEDC_TIMER_1;
|
||||
timer.freq_hz = 20000; // 20 kHz — inaudible for DC motor
|
||||
timer.clk_cfg = LEDC_AUTO_CLK;
|
||||
ledc_timer_config(&timer);
|
||||
|
||||
ledc_channel_config_t ch{};
|
||||
ch.speed_mode = LEDC_LOW_SPEED_MODE;
|
||||
ch.channel = LEDC_CHANNEL_1;
|
||||
ch.timer_sel = LEDC_TIMER_1;
|
||||
ch.gpio_num = PIN_ACTUATOR_PWM;
|
||||
ch.duty = 0;
|
||||
ledc_channel_config(&ch);
|
||||
|
||||
AR_LOGI("ElectricActuator", "init OK, limit=%.1f deg", _limit_deg);
|
||||
}
|
||||
|
||||
void power_on() override { digitalWrite(PIN_DO3, HIGH); _powered = true; }
|
||||
void power_off() override { _stop_outputs(); digitalWrite(PIN_DO3, LOW); _powered = false; }
|
||||
|
||||
bool command(float angle_deg) override {
|
||||
if (!_powered || _at_standby()) return false;
|
||||
|
||||
const float clamped = std::clamp(angle_deg, -_limit_deg, _limit_deg);
|
||||
if (clamped > 0.0f && _limit_stbd()) { _stop_outputs(); return false; }
|
||||
if (clamped < 0.0f && _limit_port()) { _stop_outputs(); return false; }
|
||||
|
||||
const float raw_pct = std::fabsf(clamped) / _limit_deg * 100.0f;
|
||||
// Apply deadband: below deadband -> 0, otherwise scale to [deadband, 100]
|
||||
const float db = AR_ACTUATOR_DEADBAND_PCT;
|
||||
const float duty_pct = (raw_pct < 0.5f) ? 0.0f
|
||||
: db + raw_pct * (100.0f - db) / 100.0f;
|
||||
|
||||
_set_pwm(static_cast<uint8_t>(std::min(duty_pct, 100.0f) * 255.0f / 100.0f));
|
||||
|
||||
if (clamped > 0.01f) {
|
||||
digitalWrite(PIN_DO1, LOW); digitalWrite(PIN_DO2, HIGH);
|
||||
} else if (clamped < -0.01f) {
|
||||
digitalWrite(PIN_DO2, LOW); digitalWrite(PIN_DO1, HIGH);
|
||||
} else {
|
||||
_stop_outputs();
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
float actual_angle_deg() const override { return rudder_sensor_get_angle_deg(); }
|
||||
bool is_powered() const override { return _powered; }
|
||||
bool at_limit() const override { return _limit_port() || _limit_stbd(); }
|
||||
float angle_limit_deg() const override { return _limit_deg; }
|
||||
ActuatorType type() const override { return ActuatorType::ELECTRIC_DC; }
|
||||
|
||||
void set_angle_limit_deg(float limit_deg) override {
|
||||
_limit_deg = std::clamp(limit_deg, MIN_LIMIT_DEG, MAX_LIMIT_DEG);
|
||||
_prefs.begin(NVS_NS, false);
|
||||
_prefs.putFloat(NVS_KEY, _limit_deg);
|
||||
_prefs.end();
|
||||
AR_LOGI("ElectricActuator", "angle_limit updated -> %.1f deg", _limit_deg);
|
||||
}
|
||||
|
||||
private:
|
||||
Preferences _prefs;
|
||||
float _limit_deg{DEFAULT_LIMIT_DEG};
|
||||
bool _powered{false};
|
||||
|
||||
static bool _limit_port() { return digitalRead(PIN_DI2) == HIGH; }
|
||||
static bool _limit_stbd() { return digitalRead(PIN_DI3) == HIGH; }
|
||||
static bool _at_standby();
|
||||
|
||||
void _stop_outputs() {
|
||||
_set_pwm(0);
|
||||
digitalWrite(PIN_DO1, LOW);
|
||||
digitalWrite(PIN_DO2, LOW);
|
||||
}
|
||||
|
||||
static void _set_pwm(uint8_t duty) {
|
||||
ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_1, duty);
|
||||
ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_1);
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace arautopilot::hal
|
||||
@@ -0,0 +1,52 @@
|
||||
// =============================================================================
|
||||
// rudder_actuator_factory.h -- selects concrete actuator at build time
|
||||
// =============================================================================
|
||||
//
|
||||
// The AR_ACTUATOR_TYPE macro is defined in firmware_config.h (generated by
|
||||
// the .appack compiler). If firmware_config.h is not present (standalone
|
||||
// build), falls back to ACTUATOR_HYDRAULIC_REVERSIBLE.
|
||||
//
|
||||
// Usage in main.cpp:
|
||||
//
|
||||
// #include "hal/rudder_actuator_factory.h"
|
||||
// // ...
|
||||
// IRudderActuator& actuator = arautopilot::hal::g_rudder_actuator;
|
||||
// actuator.init();
|
||||
// =============================================================================
|
||||
|
||||
#pragma once
|
||||
|
||||
// Pull in generated config if available
|
||||
#if __has_include("firmware_config.h")
|
||||
# include "firmware_config.h"
|
||||
#endif
|
||||
|
||||
// Actuator type constants (must match _actuator_type_to_enum in appack.py)
|
||||
#define ACTUATOR_HYDRAULIC_REVERSIBLE 0
|
||||
#define ACTUATOR_ELECTRIC_DC 1
|
||||
#define ACTUATOR_SERVOMOTOR 2
|
||||
#define ACTUATOR_STERNDRIVE_ANALOG 3
|
||||
#define ACTUATOR_TEST_RIG_MOTOR 4
|
||||
|
||||
#ifndef AR_ACTUATOR_TYPE
|
||||
# define AR_ACTUATOR_TYPE ACTUATOR_HYDRAULIC_REVERSIBLE
|
||||
#endif
|
||||
|
||||
#if AR_ACTUATOR_TYPE == ACTUATOR_HYDRAULIC_REVERSIBLE
|
||||
# include "hal/rudder_actuator_hydraulic.h"
|
||||
namespace arautopilot::hal {
|
||||
static HydraulicRudderActuator g_rudder_actuator;
|
||||
}
|
||||
#elif AR_ACTUATOR_TYPE == ACTUATOR_ELECTRIC_DC
|
||||
# include "hal/rudder_actuator_electric.h"
|
||||
namespace arautopilot::hal {
|
||||
static ElectricDcRudderActuator g_rudder_actuator;
|
||||
}
|
||||
#else
|
||||
// Fallback: hydraulic (compile warning so the integrator notices)
|
||||
# pragma message("AR_ACTUATOR_TYPE not recognised — defaulting to HydraulicRudderActuator")
|
||||
# include "hal/rudder_actuator_hydraulic.h"
|
||||
namespace arautopilot::hal {
|
||||
static HydraulicRudderActuator g_rudder_actuator;
|
||||
}
|
||||
#endif
|
||||
@@ -0,0 +1,148 @@
|
||||
// =============================================================================
|
||||
// rudder_actuator_hydraulic.h -- reversible hydraulic pump actuator (Sprint 4)
|
||||
// =============================================================================
|
||||
//
|
||||
// Drives a reversible hydraulic pump (Hynautic, Hypro, Octopus, Vetus,
|
||||
// Lecomble & Schmitt) via:
|
||||
// DO1 (GPIO) -- port direction solenoid
|
||||
// DO2 (GPIO) -- starboard direction solenoid
|
||||
// DO3 (GPIO) -- master power relay
|
||||
// PWM channel -- pump speed proportional to |angle_error|
|
||||
//
|
||||
// The software angle limit is loaded from NVS at init() and clamped to
|
||||
// [15, 45] degrees. It can be updated live via set_angle_limit_deg()
|
||||
// which also persists the new value to NVS.
|
||||
// =============================================================================
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
|
||||
#include <Preferences.h>
|
||||
#include <driver/ledc.h>
|
||||
|
||||
#include "hal/di_do.h"
|
||||
#include "hal/pinout.h"
|
||||
#include "hal/rudder_actuator_iface.h"
|
||||
#include "hal/rudder_sensor.h"
|
||||
#include "system/ar_log.h"
|
||||
|
||||
namespace arautopilot::hal {
|
||||
|
||||
class HydraulicRudderActuator final : public IRudderActuator {
|
||||
public:
|
||||
// NVS namespace + key for the angle limit
|
||||
static constexpr const char* NVS_NS = "ar_act";
|
||||
static constexpr const char* NVS_KEY = "angle_limit";
|
||||
static constexpr float DEFAULT_LIMIT_DEG = 35.0f;
|
||||
static constexpr float MIN_LIMIT_DEG = 15.0f;
|
||||
static constexpr float MAX_LIMIT_DEG = 45.0f;
|
||||
|
||||
void init() override {
|
||||
// Load angle limit from NVS (falls back to firmware_config.h default)
|
||||
_prefs.begin(NVS_NS, false);
|
||||
_limit_deg = _prefs.getFloat(NVS_KEY, DEFAULT_LIMIT_DEG);
|
||||
_limit_deg = std::clamp(_limit_deg, MIN_LIMIT_DEG, MAX_LIMIT_DEG);
|
||||
_prefs.end();
|
||||
|
||||
// Configure direction + power GPIOs
|
||||
pinMode(PIN_DO1, OUTPUT); digitalWrite(PIN_DO1, LOW); // port
|
||||
pinMode(PIN_DO2, OUTPUT); digitalWrite(PIN_DO2, LOW); // starboard
|
||||
pinMode(PIN_DO3, OUTPUT); digitalWrite(PIN_DO3, LOW); // master power
|
||||
|
||||
// Configure LEDC PWM for pump speed
|
||||
ledc_timer_config_t timer{};
|
||||
timer.speed_mode = LEDC_LOW_SPEED_MODE;
|
||||
timer.duty_resolution = LEDC_TIMER_8_BIT;
|
||||
timer.timer_num = LEDC_TIMER_0;
|
||||
timer.freq_hz = 5000;
|
||||
timer.clk_cfg = LEDC_AUTO_CLK;
|
||||
ledc_timer_config(&timer);
|
||||
|
||||
ledc_channel_config_t ch{};
|
||||
ch.speed_mode = LEDC_LOW_SPEED_MODE;
|
||||
ch.channel = LEDC_CHANNEL_0;
|
||||
ch.timer_sel = LEDC_TIMER_0;
|
||||
ch.gpio_num = PIN_ACTUATOR_PWM;
|
||||
ch.duty = 0;
|
||||
ledc_channel_config(&ch);
|
||||
|
||||
AR_LOGI("HydraulicActuator", "init OK, limit=%.1f deg", _limit_deg);
|
||||
}
|
||||
|
||||
void power_on() override {
|
||||
digitalWrite(PIN_DO3, HIGH);
|
||||
_powered = true;
|
||||
}
|
||||
|
||||
void power_off() override {
|
||||
_stop_outputs();
|
||||
digitalWrite(PIN_DO3, LOW);
|
||||
_powered = false;
|
||||
}
|
||||
|
||||
bool command(float angle_deg) override {
|
||||
if (!_powered) return false;
|
||||
if (_at_standby()) return false;
|
||||
|
||||
// Software angle limit
|
||||
const float clamped = std::clamp(angle_deg, -_limit_deg, _limit_deg);
|
||||
|
||||
// Hard-stop if limit switch asserted in the commanded direction
|
||||
if (clamped > 0.0f && _limit_stbd()) { _stop_outputs(); return false; }
|
||||
if (clamped < 0.0f && _limit_port()) { _stop_outputs(); return false; }
|
||||
|
||||
const float duty_pct = std::fabsf(clamped) / _limit_deg * 100.0f;
|
||||
_set_pwm(static_cast<uint8_t>(duty_pct * 255.0f / 100.0f));
|
||||
|
||||
if (clamped > 0.01f) {
|
||||
digitalWrite(PIN_DO1, LOW); digitalWrite(PIN_DO2, HIGH);
|
||||
} else if (clamped < -0.01f) {
|
||||
digitalWrite(PIN_DO2, LOW); digitalWrite(PIN_DO1, HIGH);
|
||||
} else {
|
||||
_stop_outputs();
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
float actual_angle_deg() const override {
|
||||
return rudder_sensor_get_angle_deg();
|
||||
}
|
||||
|
||||
bool is_powered() const override { return _powered; }
|
||||
bool at_limit() const override { return _limit_port() || _limit_stbd(); }
|
||||
float angle_limit_deg() const override { return _limit_deg; }
|
||||
ActuatorType type() const override { return ActuatorType::HYDRAULIC_REVERSIBLE; }
|
||||
|
||||
void set_angle_limit_deg(float limit_deg) override {
|
||||
_limit_deg = std::clamp(limit_deg, MIN_LIMIT_DEG, MAX_LIMIT_DEG);
|
||||
_prefs.begin(NVS_NS, false);
|
||||
_prefs.putFloat(NVS_KEY, _limit_deg);
|
||||
_prefs.end();
|
||||
AR_LOGI("HydraulicActuator", "angle_limit updated -> %.1f deg", _limit_deg);
|
||||
}
|
||||
|
||||
private:
|
||||
Preferences _prefs;
|
||||
float _limit_deg{DEFAULT_LIMIT_DEG};
|
||||
bool _powered{false};
|
||||
|
||||
static bool _limit_port() { return digitalRead(PIN_DI2) == HIGH; }
|
||||
static bool _limit_stbd() { return digitalRead(PIN_DI3) == HIGH; }
|
||||
static bool _at_standby(); // defined in modes/standby.cpp
|
||||
|
||||
void _stop_outputs() {
|
||||
_set_pwm(0);
|
||||
digitalWrite(PIN_DO1, LOW);
|
||||
digitalWrite(PIN_DO2, LOW);
|
||||
}
|
||||
|
||||
static void _set_pwm(uint8_t duty) {
|
||||
ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0, duty);
|
||||
ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0);
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace arautopilot::hal
|
||||
@@ -0,0 +1,84 @@
|
||||
// =============================================================================
|
||||
// rudder_actuator_iface.h -- abstract rudder actuator interface (Sprint 4)
|
||||
// =============================================================================
|
||||
//
|
||||
// The PID inner loop knows only about IRudderActuator. The concrete
|
||||
// implementation (hydraulic, electric-DC, servo, or test-rig motor) is
|
||||
// selected at build time via the AR_ACTUATOR_TYPE macro produced by the
|
||||
// .appack firmware_config.h.
|
||||
//
|
||||
// Safety invariants (all implementations must honour):
|
||||
// 1. angle_limit_deg -- software limit loaded from NVS (or firmware_config.h
|
||||
// default). Commanded angles are clamped before
|
||||
// reaching the physical output.
|
||||
// 2. Limit switches -- DI2 (port) / DI3 (starboard) block motion in
|
||||
// the corresponding direction regardless of command.
|
||||
// 3. Master power -- DO3 must be HIGH before any motion.
|
||||
// 4. STANDBY guard -- implementations check current_mode() == STANDBY
|
||||
// and refuse motion.
|
||||
// =============================================================================
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
namespace arautopilot::hal {
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Actuator type enumeration (mirrors .appack firmware_config.h values)
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
enum class ActuatorType : uint8_t {
|
||||
HYDRAULIC_REVERSIBLE = 0, ///< Reversible hydraulic pump (Hynautic, Hypro …)
|
||||
ELECTRIC_DC = 1, ///< Reversible DC motor with mechanical end-stops
|
||||
SERVOMOTOR = 2, ///< Servomotor with built-in position feedback
|
||||
STERNDRIVE_ANALOG = 3, ///< Analog directional sterndrive
|
||||
TEST_RIG_MOTOR = 4, ///< HIL test rig: DC motor rotates compass platform
|
||||
};
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Abstract interface
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
class IRudderActuator {
|
||||
public:
|
||||
virtual ~IRudderActuator() = default;
|
||||
|
||||
/// One-time hardware initialisation (GPIOs, PWM channels …).
|
||||
virtual void init() = 0;
|
||||
|
||||
/// Engage master power relay. Required before command().
|
||||
virtual void power_on() = 0;
|
||||
|
||||
/// Drop master power relay and stop all outputs immediately.
|
||||
virtual void power_off() = 0;
|
||||
|
||||
/// Command the actuator toward *angle_deg* (positive = starboard).
|
||||
/// The implementation MUST clamp to [-angle_limit_deg, +angle_limit_deg]
|
||||
/// before driving any output.
|
||||
/// Returns true if the command was applied; false if an interlock blocked it.
|
||||
virtual bool command(float angle_deg) = 0;
|
||||
|
||||
/// Estimated actual rudder angle (degrees) from the sensor/feedback.
|
||||
/// Returns 0.0 if no feedback is available.
|
||||
virtual float actual_angle_deg() const = 0;
|
||||
|
||||
/// True if master power relay is currently energised.
|
||||
virtual bool is_powered() const = 0;
|
||||
|
||||
/// True if either limit switch is asserted.
|
||||
virtual bool at_limit() const = 0;
|
||||
|
||||
/// Current software angle limit (degrees, positive value).
|
||||
/// Can be updated at runtime via set_angle_limit_deg().
|
||||
virtual float angle_limit_deg() const = 0;
|
||||
|
||||
/// Update the software angle limit. Clamped to [15, 45] degrees.
|
||||
/// Persisted to NVS by the concrete implementation.
|
||||
virtual void set_angle_limit_deg(float limit_deg) = 0;
|
||||
|
||||
/// Actuator type tag for diagnostics.
|
||||
virtual ActuatorType type() const = 0;
|
||||
};
|
||||
|
||||
} // namespace arautopilot::hal
|
||||
Reference in New Issue
Block a user