diff --git a/Firmware/Board/v3/Inc/board.h b/Firmware/Board/v3/Inc/board.h index 3443b90c..4820b93b 100644 --- a/Firmware/Board/v3/Inc/board.h +++ b/Firmware/Board/v3/Inc/board.h @@ -41,6 +41,8 @@ #define DEFAULT_BRAKE_RESISTANCE (0.47f) // [ohm] #endif +#define DEFAULT_ERROR_PIN 0 + #define DEFAULT_GPIO_MODES \ ODriveIntf::GPIO_MODE_DIGITAL, \ ODriveIntf::GPIO_MODE_UART_A, \ diff --git a/Firmware/Board/v3/board.cpp b/Firmware/Board/v3/board.cpp index dada4c12..bfc56bea 100644 --- a/Firmware/Board/v3/board.cpp +++ b/Firmware/Board/v3/board.cpp @@ -513,7 +513,9 @@ void ControlLoop_IRQHandler(void) { // By this time the ADCs for both M0 and M1 should have fired again. But // let's wait for them just to be sure. - while (!(ADC2->SR & ADC_SR_EOC)); + MEASURE_TIME(odrv.task_times_.dc_calib_wait) { + while (!(ADC2->SR & ADC_SR_EOC)); + } if (!fetch_and_reset_adcs(¤t0, ¤t1)) { motors[0].disarm_with_error(Motor::ERROR_BAD_TIMING); diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 49a91efd..c2342ebe 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -151,6 +151,17 @@ void ODrive::enter_dfu_mode() { } } +bool ODrive::any_error() { + return error_ != ODrive::ERROR_NONE + || std::any_of(axes.begin(), axes.end(), [](Axis& axis){ + return axis.error_ != Axis::ERROR_NONE + || axis.motor_.error_ != Motor::ERROR_NONE + || axis.sensorless_estimator_.error_ != SensorlessEstimator::ERROR_NONE + || axis.encoder_.error_ != Encoder::ERROR_NONE + || axis.controller_.error_ != Controller::ERROR_NONE; + }); +} + void ODrive::clear_errors() { for (auto& axis: axes) { axis.motor_.error_ = Motor::ERROR_NONE; @@ -366,6 +377,8 @@ void ODrive::control_loop_cb(uint32_t timestamp) { MEASURE_TIME(axis.task_times_.current_controller_update) axis.motor_.current_control_.update(timestamp); // uses the output of controller_ or open_loop_contoller_ and encoder_ or sensorless_estimator_ or async_estimator_ } + + get_gpio(odrv.config_.error_gpio_pin).write(odrv.any_error()); } @@ -407,6 +420,14 @@ uint32_t ODrive::get_dma_status(uint8_t stream_num) { return (is_reset ? 0 : 0x80000000) | ((channel & 0x7) << 2) | (priority & 0x3); } +uint32_t ODrive::get_gpio_states() { + // TODO: get values that were sampled synchronously with the control loop + uint32_t val = 0; + for (size_t i = 0; i < GPIO_COUNT; ++i) { + val |= ((gpios[i].read() ? 1UL : 0UL) << i); + } + return val; +} /** * @brief Main thread started from main(). @@ -580,6 +601,7 @@ extern "C" int main(void) { mode == ODriveIntf::GPIO_MODE_DIGITAL_PULL_UP || mode == ODriveIntf::GPIO_MODE_DIGITAL_PULL_DOWN || mode == ODriveIntf::GPIO_MODE_MECH_BRAKE || + mode == ODriveIntf::GPIO_MODE_STATUS || mode == ODriveIntf::GPIO_MODE_ANALOG_IN) { GPIO_InitStruct.Alternate = 0; } else { @@ -681,6 +703,11 @@ extern "C" int main(void) { GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; } break; + case ODriveIntf::GPIO_MODE_STATUS: { + GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + } break; default: { odrv.misconfigured_ = true; continue; diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index a81cdc2a..be41bd3b 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -256,7 +256,7 @@ void Motor::apply_pwm_timings(uint16_t timings[3], bool tentative) { * arm() is called. */ bool Motor::disarm(bool* p_was_armed) { - bool was_armed; + bool was_armed = false; CRITICAL_SECTION() { was_armed = is_armed_; diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index 88549e74..c88618bf 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -104,6 +104,7 @@ struct BoardConfig_t { float dc_max_positive_current = INFINITY; // Max current [A] the power supply can source float dc_max_negative_current = -0.000001f; // Max current [A] the power supply can sink. You most likely want a non-positive value here. Set to -INFINITY to disable. + uint32_t error_gpio_pin = DEFAULT_ERROR_PIN; PWMMapping_t pwm_mappings[4]; PWMMapping_t analog_mappings[GPIO_COUNT]; }; @@ -112,6 +113,7 @@ struct TaskTimes { TaskTimer sampling; TaskTimer control_loop_misc; TaskTimer control_loop_checks; + TaskTimer dc_calib_wait; }; @@ -169,6 +171,7 @@ public: void erase_configuration() override; void reboot() override { NVIC_SystemReset(); } void enter_dfu_mode() override; + bool any_error(); void clear_errors() override; float get_adc_voltage(uint32_t gpio) override { @@ -189,6 +192,7 @@ public: uint32_t get_interrupt_status(int32_t irqn); uint32_t get_dma_status(uint8_t stream_num); + uint32_t get_gpio_states(); void disarm_with_error(Error error); Error error_ = ERROR_NONE; @@ -230,10 +234,12 @@ public: bool& brake_resistor_saturated_ = ::brake_resistor_saturated; // TODO: make this the actual variable SystemStats_t system_stats_; + + // Edit these to suit your capture needs Oscilloscope oscilloscope_{ - &axes[0].motor_.current_control_.v_current_control_integral_d_, // trigger_src + nullptr, // trigger_src 0.5f, // trigger_threshold - nullptr // &axes[0].motor_.current_control_.Ialpha_measured_ // data_src TODO: change data type + nullptr // data_src TODO: change data type }; BoardConfig_t config_; diff --git a/Firmware/MotorControl/oscilloscope.cpp b/Firmware/MotorControl/oscilloscope.cpp index 05d2038f..21320fd6 100644 --- a/Firmware/MotorControl/oscilloscope.cpp +++ b/Firmware/MotorControl/oscilloscope.cpp @@ -5,25 +5,23 @@ #define OSCILLOSCOPE_SIZE 4096 void Oscilloscope::update() { - // Edit these to suit your capture needs float trigger_data = trigger_src_ ? *trigger_src_ : 0.0f; float trigger_threshold = trigger_threshold_; - float sample_data = data_src_ ? *data_src_ : 0.0f; + float sample_data = data_src_ ? **data_src_ : 0.0f; - static bool ready = false; - static bool capturing = false; if (trigger_data < trigger_threshold) { - ready = true; + ready_ = true; } - if (ready && trigger_data >= trigger_threshold) { - capturing = true; - ready = false; + if (ready_ && trigger_data >= trigger_threshold) { + capturing_ = true; + ready_ = false; } - if (capturing) { - data_[pos_] = sample_data; - if (++pos_ >= OSCILLOSCOPE_SIZE) { + if (capturing_) { + if (pos_ < OSCILLOSCOPE_SIZE) { + data_[pos_++] = sample_data; + } else { pos_ = 0; - capturing = false; + capturing_ = false; } } } diff --git a/Firmware/MotorControl/oscilloscope.hpp b/Firmware/MotorControl/oscilloscope.hpp index b1a5e016..df6dcd2a 100644 --- a/Firmware/MotorControl/oscilloscope.hpp +++ b/Firmware/MotorControl/oscilloscope.hpp @@ -8,7 +8,7 @@ class Oscilloscope : public ODriveIntf::OscilloscopeIntf { public: - Oscilloscope(float* trigger_src, float trigger_threshold, float* data_src) + Oscilloscope(float* trigger_src, float trigger_threshold, float** data_src) : trigger_src_(trigger_src), trigger_threshold_(trigger_threshold), data_src_(data_src) {} float get_val(uint32_t index) override { @@ -20,10 +20,12 @@ public: const uint32_t size_ = OSCILLOSCOPE_SIZE; const float* trigger_src_; const float trigger_threshold_; - const float* data_src_; + float* const * data_src_; float data_[OSCILLOSCOPE_SIZE] = {0}; size_t pos_ = 0; + bool ready_ = false; + bool capturing_ = false; }; #endif // __OSCILLOSCOPE_HPP \ No newline at end of file diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml index f4ba60b5..bc982ceb 100644 --- a/Firmware/odrive-interface.yaml +++ b/Firmware/odrive-interface.yaml @@ -122,6 +122,7 @@ interfaces: sampling: TaskTimer control_loop_misc: TaskTimer control_loop_checks: TaskTimer + dc_calib_wait: TaskTimer system_stats: c_is_class: False attributes: @@ -311,10 +312,12 @@ interfaces: brief: Max current the power supply can sink. doc: You most likely want a non-positive value here. Set to -INFINITY to disable. - gpio1_pwm_mapping: {type: Endpoint, c_name: 'pwm_mappings[0]', doc: Make sure the corresponding GPIO is in `GPIO_MODE_PWM`.} - gpio2_pwm_mapping: {type: Endpoint, c_name: 'pwm_mappings[1]', doc: Make sure the corresponding GPIO is in `GPIO_MODE_PWM`.} - gpio3_pwm_mapping: {type: Endpoint, c_name: 'pwm_mappings[2]', doc: Make sure the corresponding GPIO is in `GPIO_MODE_PWM`.} - gpio4_pwm_mapping: {type: Endpoint, c_name: 'pwm_mappings[3]', doc: Make sure the corresponding GPIO is in `GPIO_MODE_PWM`.} + error_gpio_pin: {type: uint32} + + gpio1_pwm_mapping: {type: Endpoint, c_name: 'pwm_mappings[0]', doc: Make sure the corresponding GPIO is in `GPIO_MODE_PWM0`.} + gpio2_pwm_mapping: {type: Endpoint, c_name: 'pwm_mappings[1]', doc: Make sure the corresponding GPIO is in `GPIO_MODE_PWM0`.} + gpio3_pwm_mapping: {type: Endpoint, c_name: 'pwm_mappings[2]', doc: Make sure the corresponding GPIO is in `GPIO_MODE_PWM0`.} + gpio4_pwm_mapping: {type: Endpoint, c_name: 'pwm_mappings[3]', doc: Make sure the corresponding GPIO is in `GPIO_MODE_PWM0`.} gpio3_analog_mapping: {type: Endpoint, c_name: 'analog_mappings[3]', doc: Make sure the corresponding GPIO is in `GPIO_MODE_ANALOG_IN`.} gpio4_analog_mapping: {type: Endpoint, c_name: 'analog_mappings[4]', doc: Make sure the corresponding GPIO is in `GPIO_MODE_ANALOG_IN`.} user_config_loaded: readonly uint32 @@ -374,6 +377,9 @@ interfaces: bits 1:0: priority (3 is highest priority) 0xffffffff if the specified number is not a valid DMA stream number. doc: Returns information about the specified DMA stream. + get_gpio_states: + out: {status: {type: uint32}} + doc: Returns the logic states of all GPIOs. Bit i represents the state of GPIOi. clear_errors: doc: Clear all the errors of this device including all contained submodules. @@ -1122,6 +1128,7 @@ valuetypes: Enc1: {doc: The pin is used by quadrature encoder 1.} Enc2: {doc: This mode is not supported on ODrive v3.x.} MechBrake: {doc: This is to support external mechanical brakes.} + Status: {doc: The pin is used for status output (see `config.error_gpio_pin`)} ODrive.Can.Protocol: values: {Simple: } diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py index 92620a0a..79649de8 100644 --- a/tools/odrive/enums.py +++ b/tools/odrive/enums.py @@ -19,6 +19,7 @@ GPIO_MODE_ENC0 = 11 GPIO_MODE_ENC1 = 12 GPIO_MODE_ENC2 = 13 GPIO_MODE_MECH_BRAKE = 14 +GPIO_MODE_STATUS = 15 # ODrive.Can.Protocol PROTOCOL_SIMPLE = 0