From b8b483c9d6d298c0c5185601987438f8cc9210fb Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 30 Mar 2018 22:32:07 -0700 Subject: [PATCH] Firmware hardening - check return code of enqueue_modulation_timings - wrap all hardware access to safety critical hardware timer registers in safety_critical_ sections. These functions like the armed-ness of each PWM output to a corresponding armed state that can only be set at start or by the user - ensure NaN in any variables doesn't cause unpredictable behavior on the safety critical PWM outputs --- Firmware/Board/v3/Src/stm32f4xx_it.c | 6 +- Firmware/MotorControl/axis.cpp | 4 +- Firmware/MotorControl/axis.hpp | 16 +- Firmware/MotorControl/communication.cpp | 1 + Firmware/MotorControl/controller.cpp | 2 +- Firmware/MotorControl/encoder.cpp | 12 +- Firmware/MotorControl/encoder.hpp | 2 +- Firmware/MotorControl/legacy_commands.h | 4 +- Firmware/MotorControl/low_level.cpp | 217 +++++++++++++++++++++--- Firmware/MotorControl/low_level.h | 14 +- Firmware/MotorControl/motor.cpp | 49 +++--- Firmware/MotorControl/motor.hpp | 17 +- Firmware/MotorControl/odrive_main.hpp | 2 + Firmware/MotorControl/protocol.cpp | 2 +- Firmware/MotorControl/utils.c | 16 +- Firmware/Tupfile.lua | 7 +- Firmware/build.sh | 3 + 17 files changed, 285 insertions(+), 89 deletions(-) diff --git a/Firmware/Board/v3/Src/stm32f4xx_it.c b/Firmware/Board/v3/Src/stm32f4xx_it.c index 0a940b15..cd96644a 100644 --- a/Firmware/Board/v3/Src/stm32f4xx_it.c +++ b/Firmware/Board/v3/Src/stm32f4xx_it.c @@ -38,11 +38,15 @@ /* USER CODE BEGIN 0 */ #include "freertos_vars.h" -#include "low_level.h" +#include typedef void (*ADC_handler_t)(ADC_HandleTypeDef* hadc, bool injected); void ADC_IRQ_Dispatch(ADC_HandleTypeDef* hadc, ADC_handler_t callback); +// TODO: move somewhere else +void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected); +void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected); + /* USER CODE END 0 */ /* External variables --------------------------------------------------------*/ diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index d6c718e4..f9696284 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -53,9 +53,7 @@ void Axis::signal_current_meas() { // @brief Blocks until a current measurement is completed // @returns True on success, false otherwise bool Axis::wait_for_current_meas() { - if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) - return error_ = ERROR_CURRENT_MEASUREMENT_TIMEOUT, false; - return true; + return osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status == osEventSignal; } static void step_cb_wrapper(void* ctx) { diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 300c7e94..c7f909e0 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -106,7 +106,8 @@ public: error_ = ERROR_MOTOR_FAILED; break; } - if ((current_state_ != AXIS_STATE_IDLE) && missed_control_deadline_) { + if ((current_state_ != AXIS_STATE_IDLE) && (motor_.armed_state_ == Motor::ARMED_STATE_DISARMED)) { + // motor got disarmed in something other than the idle loop error_ = ERROR_CONTROL_LOOP_TIMEOUT; break; } @@ -121,8 +122,12 @@ public: ++loop_counter_; // Wait until the current measurement interrupt fires - if (!wait_for_current_meas()) { // error set by function call - motor_.disarm(); // maybe the interrupt handler is dead, let's be safe and float all phases + if (!wait_for_current_meas()) { + // maybe the interrupt handler is dead, let's be + // safe and float the phases + safety_critical_disarm_motor_pwm(motor_); + update_brake_current(); + error_ = ERROR_CURRENT_MEASUREMENT_TIMEOUT; break; } } @@ -148,10 +153,6 @@ public: // variables exposed on protocol Error_t error_ = ERROR_NO_ERROR; - bool missed_control_deadline_ = true; // this flag is raised by the interrupt handler - // whenever there's no active control loop that - // sets the timings. The flag must be explicitly - // cleared by a call to motors.arm(). bool enable_step_dir_ = false; // auto enabled after calibration, based on config.enable_step_dir AxisState_t requested_state_ = AXIS_STATE_STARTUP_SEQUENCE; AxisState_t task_chain_[10] = { AXIS_STATE_UNDEFINED }; @@ -162,7 +163,6 @@ public: auto make_protocol_definitions() { return make_protocol_member_list( make_protocol_property("error", &error_), - make_protocol_ro_property("missed_control_deadline", &missed_control_deadline_), make_protocol_property("enable_step_dir", &enable_step_dir_), make_protocol_ro_property("current_state", ¤t_state_), make_protocol_property("requested_state", &requested_state_), diff --git a/Firmware/MotorControl/communication.cpp b/Firmware/MotorControl/communication.cpp index 8f951fbb..b8a0ed65 100644 --- a/Firmware/MotorControl/communication.cpp +++ b/Firmware/MotorControl/communication.cpp @@ -172,6 +172,7 @@ static inline auto make_obj_tree() { make_protocol_ro_property("vbus_voltage", &vbus_voltage), make_protocol_ro_property("comm_stack_info", &comm_stack_info), make_protocol_ro_property("serial_number", &serial_number), + make_protocol_ro_property("brake_resistor_armed", &brake_resistor_armed_), make_protocol_object("config", make_protocol_property("brake_resistance", &board_config.brake_resistance), // TODO: changing this currently requires a reboot - fix this diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index c20cf504..3e1b5661 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -96,7 +96,7 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s // We get the current position and apply a current feed-forward // ensuring that we handle negative encoder positions properly (-1 == motor->encoder.encoder_cpr - 1) if (anticogging_.use_anticogging) { - Iq += anticogging_.cogging_map[mod(pos_estimate, axis_->encoder_.config_.cpr)]; + Iq += anticogging_.cogging_map[mod(static_cast(pos_estimate), axis_->encoder_.config_.cpr)]; } float v_err = vel_des - vel_estimate; diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 37a40b1c..8fae0d95 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -80,7 +80,8 @@ bool Encoder::run_index_search() { float v_alpha = voltage_magnitude * arm_cos_f32(phase); float v_beta = voltage_magnitude * arm_sin_f32(phase); - axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta); + if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta)) + return false; // error set inside enqueue_voltage_timings axis_->motor_.log_timing(Motor::TIMING_LOG_IDX_SEARCH); // continue until the index is found @@ -115,7 +116,8 @@ bool Encoder::run_offset_calibration() { // go to motor zero phase for start_lock_duration to get ready to scan int i = 0; axis_->run_control_loop([&](){ - axis_->motor_.enqueue_voltage_timings(voltage_magnitude, 0.0f); + if (!axis_->motor_.enqueue_voltage_timings(voltage_magnitude, 0.0f)) + return false; // error set inside enqueue_voltage_timings axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB); return ++i < start_lock_duration * current_meas_hz; }); @@ -131,7 +133,8 @@ bool Encoder::run_offset_calibration() { float phase = wrap_pm_pi(scan_distance * (float)i / (float)num_steps - scan_distance / 2.0f); float v_alpha = voltage_magnitude * arm_cos_f32(phase); float v_beta = voltage_magnitude * arm_sin_f32(phase); - axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta); + if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta)) + return false; // error set inside enqueue_voltage_timings axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB); encvaluesum += (int16_t)hw_config_.timer->Instance->CNT; @@ -169,7 +172,8 @@ bool Encoder::run_offset_calibration() { float phase = wrap_pm_pi(-scan_distance * (float)i / (float)num_steps + scan_distance / 2.0f); float v_alpha = voltage_magnitude * arm_cos_f32(phase); float v_beta = voltage_magnitude * arm_sin_f32(phase); - axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta); + if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta)) + return false; // error set inside enqueue_voltage_timings axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB); encvaluesum += (int16_t)hw_config_.timer->Instance->CNT; diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index f3dbc78c..2edf5be0 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -16,7 +16,7 @@ struct EncoderConfig_t { int32_t cpr = (2048 * 4); // Default resolution of CUI-AMT102 encoder, int32_t offset = 0; // If pre_calibrated is true, this is copied into encoder.offset_ once // index search succeeds - float calib_range = 0.02; + float calib_range = 0.02f; }; class Encoder { diff --git a/Firmware/MotorControl/legacy_commands.h b/Firmware/MotorControl/legacy_commands.h index bb945dab..11ee7203 100644 --- a/Firmware/MotorControl/legacy_commands.h +++ b/Firmware/MotorControl/legacy_commands.h @@ -6,7 +6,9 @@ extern "C" { #endif /* Includes ------------------------------------------------------------------*/ -#include "low_level.h" +#include +#include +#include /* Exported types ------------------------------------------------------------*/ typedef enum { diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index b6f36d39..01f8a392 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -7,8 +7,6 @@ #define ARM_MATH_CM4 #include -#include - #include #include #include @@ -34,9 +32,166 @@ // This value is updated by the DC-bus reading ADC. // Arbitrary non-zero inital value to avoid division by zero if ADC reading is late float vbus_voltage = 12.0f; +bool brake_resistor_armed_ = false; /* Private constant data -----------------------------------------------------*/ /* Private variables ---------------------------------------------------------*/ + +/* CPU critical section helpers ----------------------------------------------*/ + +static inline uint8_t cpu_enter_critical() { + uint8_t status_register; + asm ( + "MRS R0, PRIMASK\n\t" + "CPSID I\n\t" + "STRB R0, %[output]" + : [output] "=m" (status_register) :: "r0" + ); + return status_register; +} + +static inline void cpu_exit_critical(uint8_t status_register) { + asm ( + "ldrb r0, %[input]\n\t" + "msr PRIMASK,r0;\n\t" + ::[input] "m" (status_register) : "r0" + ); +} + +/* Safety critical functions -------------------------------------------------*/ + +/* +* This section contains all accesses to safety critical hardware registers. +* Specifically, these registers: +* Motor0 PWMs: +* Timer1.MOE (master output enabled) +* Timer1.CCR1 (counter compare register 1) +* Timer1.CCR2 (counter compare register 2) +* Timer1.CCR3 (counter compare register 3) +* Motor1 PWMs: +* Timer8.MOE (master output enabled) +* Timer8.CCR1 (counter compare register 1) +* Timer8.CCR2 (counter compare register 2) +* Timer8.CCR3 (counter compare register 3) +* Brake resistor PWM: +* Timer2.CCR3 (counter compare register 3) +* Timer2.CCR4 (counter compare register 4) +* +* The following assumptions are made: +* - The hardware operates as described in the datasheet: +* http://www.st.com/content/ccc/resource/technical/document/reference_manual/3d/6d/5a/66/b4/99/40/d4/DM00031020.pdf/files/DM00031020.pdf/jcr:content/translations/en.DM00031020.pdf +* This assumption also requires for instance that there are no radiation +* caused hardware errors. +* - After startup, all variables used in this section are exclusively modified +* by the code in this section (this excludes function parameters) +* This assumption also requires that there is no memory corruption. +* - This code is compiled by a C standard compliant compiler. +* +* Furthermore: +* - Between calls to safety_critical_arm_motor_pwm and +* safety_critical_disarm_motor_pwm the motor's Ibus current is +* set to the correct value and update_brake_resistor is called +* at a high rate. +*/ + + +// @brief Kicks off the arming process of the motor. +// All calls to this function must clearly originate +// from user input. +void safety_critical_arm_motor_pwm(Motor& motor) { + uint8_t sr = cpu_enter_critical(); + if (brake_resistor_armed_) { + motor.armed_state_ = Motor::ARMED_STATE_WAITING_FOR_TIMINGS; + } + cpu_exit_critical(sr); +} + +// @brief Disarms the motor PWM. +// After calling this function, it is guaranteed that all three +// motor phases are floating and will not be enabled again until +// safety_critical_arm_motor_phases is called. +void safety_critical_disarm_motor_pwm(Motor& motor) { + uint8_t sr = cpu_enter_critical(); + motor.armed_state_ = Motor::ARMED_STATE_DISARMED; + __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(motor.hw_config_.timer); + cpu_exit_critical(sr); +} + +// @brief Updates the phase timings unless the motor is disarmed. +// +// If this is called at a rate higher than the motor's timer period, +// the actual PMW timings on the pins can be undefined for up to one +// timer period. +void safety_critical_apply_motor_pwm_timings(Motor& motor, uint16_t timings[3]) { + uint8_t sr = cpu_enter_critical(); + if (!brake_resistor_armed_) { + motor.armed_state_ = Motor::ARMED_STATE_ARMED; + } + + motor.hw_config_.timer->Instance->CCR1 = timings[0]; + motor.hw_config_.timer->Instance->CCR2 = timings[1]; + motor.hw_config_.timer->Instance->CCR3 = timings[2]; + + if (motor.armed_state_ == Motor::ARMED_STATE_WAITING_FOR_TIMINGS) { + // timings were just loaded into the timer registers + // the timer register are buffered, so they won't have an effect + // on the output just yet so we need to wait until the next + // interrupt before we actually enable the output + motor.armed_state_ = Motor::ARMED_STATE_WAITING_FOR_UPDATE; + } else if (motor.armed_state_ == Motor::ARMED_STATE_WAITING_FOR_UPDATE) { + // now we waited long enough. Enter armed state and + // enable the actual PWM outputs. + motor.armed_state_ = Motor::ARMED_STATE_ARMED; + __HAL_TIM_MOE_ENABLE(motor.hw_config_.timer); // enable pwm outputs + } else if (motor.armed_state_ == Motor::ARMED_STATE_ARMED) { + // nothing to do, PWM is running, all good + } else { + // unknown state oh no + safety_critical_disarm_motor_pwm(motor); + } + cpu_exit_critical(sr); +} + +// @brief Arms the brake resistor +void safety_critical_arm_brake_resistor() { + uint8_t sr = cpu_enter_critical(); + brake_resistor_armed_ = true; + htim2.Instance->CCR3 = 0; + htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; + cpu_exit_critical(sr); +} + +// @brief Disarms the brake resistor and by extension +// all motor PWM outputs. +// After calling this, the brake resistor can only be armed again +// by calling safety_critical_arm_brake_resistor(). +void safety_critical_disarm_brake_resistor() { + uint8_t sr = cpu_enter_critical(); + brake_resistor_armed_ = false; + htim2.Instance->CCR3 = 0; + htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; + for (size_t i = 0; i < AXIS_COUNT; ++i) { + safety_critical_disarm_motor_pwm(axes[i]->motor_); + } + cpu_exit_critical(sr); +} + +// @brief Updates the brake resistor PWM timings unless +// the brake resistor is disarmed. +void safety_critical_apply_brake_resistor_timings(uint32_t low_off, uint32_t high_on) { + uint8_t sr = cpu_enter_critical(); + if (brake_resistor_armed_) { + // Safe update of low and high side timings + // To avoid race condition, first reset timings to safe state + // ch3 is low side, ch4 is high side + htim2.Instance->CCR3 = 0; + htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; + htim2.Instance->CCR3 = low_off; + htim2.Instance->CCR4 = high_on; + } + cpu_exit_critical(sr); +} + /* Function implementations --------------------------------------------------*/ void start_adc_pwm() { @@ -70,6 +225,12 @@ void start_adc_pwm() { htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3); HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4); + + // Disarm motors and arm brake resistor + for (size_t i = 0; i < AXIS_COUNT; ++i) { + safety_critical_disarm_motor_pwm(axes[i]->motor_); + } + safety_critical_arm_brake_resistor(); } void start_pwm(TIM_HandleTypeDef* htim) { @@ -137,13 +298,15 @@ void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b, htim_b->Instance->BDTR |= MOE_store_b; } -// @brief Floats ALL phases immediately and sets the brake current to 0. -void disable_all_pwms(Motor::Error_t error) { +// @brief Floats ALL phases immediately and disarms both motors and the brake resistor. +void low_level_fault(Motor::Error_t error) { // Disable all motors NOW! for (size_t i = 0; i < AXIS_COUNT; ++i) { - axes[i]->motor_.disarm(); + safety_critical_disarm_motor_pwm(axes[i]->motor_); axes[i]->motor_.error_ = error; } + + safety_critical_disarm_brake_resistor(); } //-------------------------------- @@ -166,7 +329,7 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { // Ensure ADCs are expected ones to simplify the logic below if (!(hadc == &hadc2 || hadc == &hadc3)) { - disable_all_pwms(Motor::ERROR_ADC_FAILED); + low_level_fault(Motor::ERROR_ADC_FAILED); return; }; @@ -189,18 +352,17 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { // Load next timings for the motor that we're not currently sampling if (update_timings) { - if (other_axis.motor_.next_timings_valid_ && !other_axis.missed_control_deadline_) { - other_axis.motor_.next_timings_valid_ = false; - other_axis.motor_.hw_config_.timer->Instance->CCR1 = other_axis.motor_.next_timings_[0]; - other_axis.motor_.hw_config_.timer->Instance->CCR2 = other_axis.motor_.next_timings_[1]; - other_axis.motor_.hw_config_.timer->Instance->CCR3 = other_axis.motor_.next_timings_[2]; - __HAL_TIM_MOE_ENABLE(other_axis.motor_.hw_config_.timer); // enable pwm outputs - update_brake_current(); - } else { + if (!other_axis.motor_.next_timings_valid_) { // the motor control loop failed to update the timings in time // we must assume that it died and therefore float all phases - other_axis.motor_.disarm(); + safety_critical_disarm_motor_pwm(other_axis.motor_); + } else { + other_axis.motor_.next_timings_valid_ = false; + safety_critical_apply_motor_pwm_timings( + other_axis.motor_, other_axis.motor_.next_timings_ + ); } + update_brake_current(); } // Check the timing of the sequencing @@ -248,7 +410,9 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { void update_brake_current() { float Ibus_sum = 0.0f; for (size_t i = 0; i < AXIS_COUNT; ++i) { - Ibus_sum += axes[i]->motor_.current_control_.Ibus; + if (axes[i]->motor_.armed_state_ == Motor::ARMED_STATE_ARMED) { + Ibus_sum += axes[i]->motor_.current_control_.Ibus; + } } float brake_current = -Ibus_sum; // Clip negative values to 0.0f @@ -256,16 +420,13 @@ void update_brake_current() { float brake_duty = brake_current * board_config.brake_resistance / vbus_voltage; // Duty limit at 90% to allow bootstrap caps to charge - if (brake_duty > 0.9f) brake_duty = 0.9f; - int high_on = TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty); - int low_off = high_on - TIM_APB1_DEADTIME_CLOCKS; - if (low_off < 0) low_off = 0; - - // Safe update of low and high side timings - // To avoid race condition, first reset timings to safe state - // ch3 is low side, ch4 is high side - htim2.Instance->CCR3 = 0; - htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; - htim2.Instance->CCR3 = low_off; - htim2.Instance->CCR4 = high_on; + // If brake_duty is NaN, this expression will also evaluate to true + if ((brake_duty >= 0.0f) && (brake_duty <= 0.9f)) { + int high_on = static_cast(TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty)); + int low_off = high_on - TIM_APB1_DEADTIME_CLOCKS; + if (low_off < 0) low_off = 0; + safety_critical_apply_brake_resistor_timings(low_off, high_on); + } else { + safety_critical_disarm_brake_resistor(); + } } diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 3105bae0..5f4a7cde 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -2,6 +2,10 @@ #ifndef __LOW_LEVEL_H #define __LOW_LEVEL_H +#ifndef __ODRIVE_MAIN_HPP +#error "This file should not be included directly. Include odrive_main.hpp instead." +#endif + #ifdef __cplusplus extern "C" { #endif @@ -17,10 +21,18 @@ extern "C" { /* Exported macro ------------------------------------------------------------*/ /* Exported functions --------------------------------------------------------*/ -//Note: to control without feed forward, set feed forward terms to 0.0f. +void safety_critical_arm_motor_pwm(Motor& motor); +void safety_critical_disarm_motor_pwm(Motor& motor); +void safety_critical_apply_motor_pwm_timings(Motor& motor, uint16_t timings[3]); +void safety_critical_arm_brake_resistor(); +void safety_critical_disarm_brake_resistor(); +void safety_critical_apply_brake_resistor_timings(uint32_t low_off, uint32_t high_on); +// called from STM platform code +extern "C" { void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected); void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected); +} // Initalisation void start_adc_pwm(); diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 5979257c..cf0e80f9 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -39,25 +39,12 @@ bool Motor::arm() { // that we have exactly one full interrupt period until the third trigger. This gives // the control loop the correct time quota to set up modulation timings. if (!(axis_->wait_for_current_meas() && axis_->wait_for_current_meas())) - return false; + return axis_->error_ = Axis::ERROR_CURRENT_MEASUREMENT_TIMEOUT, false; next_timings_valid_ = false; - axis_->missed_control_deadline_ = false; + safety_critical_arm_motor_pwm(*this); return true; } -// @brief Floats the phases of this motor immediately and updates -// the brake current accordingly. -void Motor::disarm() { - // disable pwm - __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(hw_config_.timer); - // set this motor's contribution to 0 - current_control_.Ibus = 0.0f; - update_brake_current(); - // ensure the PWM is not re-enabled without the state machine explicitly - // calling motor.arm() - axis_->missed_control_deadline_ = true; -} - // @brief Tune the current controller based on phase resistance and inductance // This should be invoked whenever one of these values changes. // TODO: allow update on user-request or update automatically via hooks @@ -167,7 +154,7 @@ float Motor::phase_current_from_adcval(uint32_t ADCValue) { // TODO check Ibeta balance to verify good motor connection bool Motor::measure_phase_resistance(float test_current, float max_voltage) { static const float kI = 10.0f; // [(V/s)/A] - static const int num_test_cycles = 3.0f / CURRENT_MEAS_PERIOD; // Test runs for 3s + static const int num_test_cycles = static_cast(3.0f / CURRENT_MEAS_PERIOD); // Test runs for 3s float test_voltage = 0.0f; size_t i = 0; @@ -178,7 +165,8 @@ bool Motor::measure_phase_resistance(float test_current, float max_voltage) { return error_ = ERROR_PHASE_RESISTANCE_OUT_OF_RANGE, false; // Test voltage along phase A - enqueue_voltage_timings(test_voltage, 0.0f); + if (!enqueue_voltage_timings(test_voltage, 0.0f)) + return false; // error set inside enqueue_voltage_timings log_timing(TIMING_LOG_MEAS_R); return ++i < num_test_cycles; @@ -187,7 +175,8 @@ bool Motor::measure_phase_resistance(float test_current, float max_voltage) { return false; //// De-energize motor - //enqueue_voltage_timings(motor, 0.0f, 0.0f); + //if (!enqueue_voltage_timings(motor, 0.0f, 0.0f)) + // return false; // error set inside enqueue_voltage_timings float R = test_voltage / test_current; config_.phase_resistance = R; @@ -205,7 +194,8 @@ bool Motor::measure_phase_inductance(float voltage_low, float voltage_high) { Ialphas[i] += -current_meas_.phB - current_meas_.phC; // Test voltage along phase A - enqueue_voltage_timings(test_voltages[i], 0.0f); + if (!enqueue_voltage_timings(test_voltages[i], 0.0f)) + return false; // error set inside enqueue_voltage_timings log_timing(TIMING_LOG_MEAS_L); return ++t < (num_cycles << 1); @@ -214,7 +204,8 @@ bool Motor::measure_phase_inductance(float voltage_low, float voltage_high) { return false; //// De-energize motor - //enqueue_voltage_timings(motor, 0.0f, 0.0f); + //if (!enqueue_voltage_timings(motor, 0.0f, 0.0f)) + // return false; // error set inside enqueue_voltage_timings float v_L = 0.5f * (voltage_high - voltage_low); // Note: A more correct formula would also take into account that there is a finite timestep. @@ -251,21 +242,25 @@ bool Motor::run_calibration() { return true; } -void Motor::enqueue_modulation_timings(float mod_alpha, float mod_beta) { +bool Motor::enqueue_modulation_timings(float mod_alpha, float mod_beta) { float tA, tB, tC; - SVM(mod_alpha, mod_beta, &tA, &tB, &tC); + if (SVM(mod_alpha, mod_beta, &tA, &tB, &tC) != 0) + return error_ = ERROR_NUMERICAL, false; next_timings_[0] = (uint16_t)(tA * (float)TIM_1_8_PERIOD_CLOCKS); next_timings_[1] = (uint16_t)(tB * (float)TIM_1_8_PERIOD_CLOCKS); next_timings_[2] = (uint16_t)(tC * (float)TIM_1_8_PERIOD_CLOCKS); next_timings_valid_ = true; + return true; } -void Motor::enqueue_voltage_timings(float v_alpha, float v_beta) { +bool Motor::enqueue_voltage_timings(float v_alpha, float v_beta) { float vfactor = 1.0f / ((2.0f / 3.0f) * vbus_voltage); float mod_alpha = vfactor * v_alpha; float mod_beta = vfactor * v_beta; - enqueue_modulation_timings(mod_alpha, mod_beta); + if (!enqueue_modulation_timings(mod_alpha, mod_beta)) + return false; log_timing(TIMING_LOG_FOC_VOLTAGE); + return true; } // TODO: This doesn't update brake current @@ -275,8 +270,7 @@ bool Motor::FOC_voltage(float v_d, float v_q, float phase) { float s = arm_sin_f32(phase); float v_alpha = c*v_d - s*v_q; float v_beta = c*v_q + s*v_d; - enqueue_voltage_timings(v_alpha, v_beta); - return true; + return enqueue_voltage_timings(v_alpha, v_beta); } bool Motor::FOC_current(float Id_des, float Iq_des, float phase) { @@ -336,7 +330,8 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) { ictrl->final_v_beta = mod_to_V * mod_beta; // Apply SVM - enqueue_modulation_timings(mod_alpha, mod_beta); + if (!enqueue_modulation_timings(mod_alpha, mod_beta)) + return false; // error set inside enqueue_modulation_timings log_timing(TIMING_LOG_FOC_CURRENT); return true; diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index 37647db0..503b8756 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -60,6 +60,8 @@ public: ERROR_ADC_FAILED, ERROR_DRV_FAULT, ERROR_NOT_IMPLEMENTED_MOTOR_TYPE, + ERROR_BRAKE_CURRENT_OUT_OF_RANGE, + ERROR_NUMERICAL }; enum TimingLog_t { @@ -75,6 +77,13 @@ public: TIMING_LOG_NUM_SLOTS }; + enum ArmedState_t { + ARMED_STATE_DISARMED, + ARMED_STATE_WAITING_FOR_TIMINGS, + ARMED_STATE_WAITING_FOR_UPDATE, + ARMED_STATE_ARMED, + }; + Motor(const MotorHardwareConfig_t& hw_config, const GateDriverHardwareConfig_t& gate_driver_config, MotorConfig_t& config); @@ -94,8 +103,8 @@ public: bool measure_phase_resistance(float test_current, float max_voltage); bool measure_phase_inductance(float voltage_low, float voltage_high); bool run_calibration(); - void enqueue_modulation_timings(float mod_alpha, float mod_beta); - void enqueue_voltage_timings(float v_alpha, float v_beta); + bool enqueue_modulation_timings(float mod_alpha, float mod_beta); + bool enqueue_voltage_timings(float v_alpha, float v_beta); bool FOC_voltage(float v_d, float v_q, float phase); bool FOC_current(float Id_des, float Iq_des, float phase); bool update(float current_setpoint, float phase); @@ -120,6 +129,9 @@ public: // variables exposed on protocol Error_t error_ = ERROR_NO_ERROR; + // Do not write to this variable directly! + // It is for exclusive use by the safety_critical_... functions. + ArmedState_t armed_state_ = ARMED_STATE_DISARMED; bool is_calibrated_ = config_.pre_calibrated; Iph_BC_t current_meas_ = {0.0f, 0.0f}; Iph_BC_t DC_calib_ = {0.0f, 0.0f}; @@ -144,6 +156,7 @@ public: auto make_protocol_definitions() { return make_protocol_member_list( make_protocol_property("error", &error_), + make_protocol_ro_property("armed_state", &armed_state_), make_protocol_ro_property("is_calibrated", &is_calibrated_), make_protocol_ro_property("current_meas_phB", ¤t_meas_.phB), make_protocol_ro_property("current_meas_phC", ¤t_meas_.phC), diff --git a/Firmware/MotorControl/odrive_main.hpp b/Firmware/MotorControl/odrive_main.hpp index 730f040d..50bf69c8 100644 --- a/Firmware/MotorControl/odrive_main.hpp +++ b/Firmware/MotorControl/odrive_main.hpp @@ -26,6 +26,7 @@ struct BoardConfig_t { }; class Axis; +class Motor; //default timeout waiting for phase measurement signals #define PH_CURRENT_MEAS_TIMEOUT 2 // [ms] @@ -33,6 +34,7 @@ class Axis; static const float current_meas_period = CURRENT_MEAS_PERIOD; static const int current_meas_hz = CURRENT_MEAS_HZ; extern float vbus_voltage; +extern bool brake_resistor_armed_; extern const float elec_rad_per_enc; extern BoardConfig_t board_config; diff --git a/Firmware/MotorControl/protocol.cpp b/Firmware/MotorControl/protocol.cpp index c0cd07f0..50e38fab 100644 --- a/Firmware/MotorControl/protocol.cpp +++ b/Firmware/MotorControl/protocol.cpp @@ -1,7 +1,7 @@ /* Includes ------------------------------------------------------------------*/ -#include "low_level.h" +//#include "low_level.h" #include "protocol.hpp" #include diff --git a/Firmware/MotorControl/utils.c b/Firmware/MotorControl/utils.c index 269d1e7e..0c80088e 100644 --- a/Firmware/MotorControl/utils.c +++ b/Firmware/MotorControl/utils.c @@ -120,16 +120,12 @@ int SVM(float alpha, float beta, float* tA, float* tB, float* tC) { } } - int retval = 0; - if ( - *tA < 0.0f - || *tA > 1.0f - || *tB < 0.0f - || *tB > 1.0f - || *tC < 0.0f - || *tC > 1.0f - ) retval = -1; - return retval; + // if any of the results becomes NaN, result_valid will evaluate to false + int result_valid = + *tA >= 0.0f && *tA <= 1.0f + && *tB >= 0.0f && *tB <= 1.0f + && *tC >= 0.0f && *tC <= 1.0f; + return result_valid ? 0 : -1; } //beware of inserting large angles! diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua index d71b2238..51a35e76 100644 --- a/Firmware/Tupfile.lua +++ b/Firmware/Tupfile.lua @@ -63,6 +63,11 @@ if tup.getconfig("STEP_DIR") == "y" then end end +-- Compiler settings +if tup.getconfig("STRICT") == "true" then + FLAGS += '-Werror' +end + -- C-specific flags FLAGS += '-D__weak="__attribute__((weak))"' @@ -74,7 +79,7 @@ FLAGS += '-mthumb' FLAGS += '-mcpu=cortex-m4' FLAGS += '-mfpu=fpv4-sp-d16' FLAGS += '-mfloat-abi=hard' -FLAGS += { '-Wall', '-fdata-sections', '-ffunction-sections'} +FLAGS += { '-Wall', '-Wfloat-conversion', '-fdata-sections', '-ffunction-sections'} -- debug build FLAGS += '-g -gdwarf-2' diff --git a/Firmware/build.sh b/Firmware/build.sh index 811a4ab7..1622fa69 100755 --- a/Firmware/build.sh +++ b/Firmware/build.sh @@ -8,6 +8,9 @@ set -euo pipefail THIS_DIR="$(dirname "$0")" cd "$THIS_DIR" +# Treat warnings as errors +export CONFIG_STRICT=true + # Write all environment variables that start with "CONFIG_" to tup.config rm -rdf build mkdir -p build