From c2bf296d413846495535cf3c4ff7464d0ef43f88 Mon Sep 17 00:00:00 2001 From: Phil Tokumaru Date: Sun, 30 Jun 2024 15:42:06 -0700 Subject: [PATCH] Add DSHOT support and some other changes --- include/board.h | 2 + include/mixer.h | 277 ++++++++++-------- src/comm_manager.cpp | 608 -------------------------------------- src/command_manager.cpp | 285 ------------------ src/controller.cpp | 290 ------------------- src/estimator.cpp | 376 ------------------------ src/main.cpp | 77 ----- src/mixer.cpp | 207 ------------- src/param.cpp | 360 ----------------------- src/rc.cpp | 282 ------------------ src/rosflight.cpp | 141 --------- src/sensors.cpp | 626 ---------------------------------------- src/state_manager.cpp | 343 ---------------------- 13 files changed, 155 insertions(+), 3719 deletions(-) delete mode 100644 src/comm_manager.cpp delete mode 100644 src/command_manager.cpp delete mode 100644 src/controller.cpp delete mode 100644 src/estimator.cpp delete mode 100644 src/main.cpp delete mode 100644 src/mixer.cpp delete mode 100644 src/param.cpp delete mode 100644 src/rc.cpp delete mode 100644 src/rosflight.cpp delete mode 100644 src/sensors.cpp delete mode 100644 src/state_manager.cpp diff --git a/include/board.h b/include/board.h index 2b350b97..8d17c38a 100644 --- a/include/board.h +++ b/include/board.h @@ -116,8 +116,10 @@ class Board // PWM virtual void pwm_init(uint32_t refresh_rate, uint16_t idle_pwm) = 0; + virtual void pwm_init(const float *rate, uint32_t channels) = 0; // PTT new virtual void pwm_disable() = 0; virtual void pwm_write(uint8_t channel, float value) = 0; + virtual void pwm_write(float *value, uint32_t channels) = 0; //PTT new // non-volatile memory virtual void memory_init() = 0; diff --git a/include/mixer.h b/include/mixer.h index d11359b6..6f30e917 100644 --- a/include/mixer.h +++ b/include/mixer.h @@ -45,7 +45,7 @@ class Mixer : public ParamListenerInterface { public: static constexpr uint8_t NUM_TOTAL_OUTPUTS = 14; - static constexpr uint8_t NUM_MIXER_OUTPUTS = 8; + static constexpr uint8_t NUM_MIXER_OUTPUTS = 10; enum { @@ -62,27 +62,36 @@ class Mixer : public ParamListenerInterface FIXEDWING = 10, PASSTHROUGH = 11, VTAIL = 12, - CUSTOM = 13, + QUADPLANE = 13, + CUSTOM = 14, NUM_MIXERS, INVALID_MIXER = 255 }; - typedef enum + typedef enum :uint8_t { - NONE, // None - S, // Servo - M, // Motor - G // GPIO + NONE = 0, // None + S = 1, // Servo + M = 2, // Motor + // G // GPIO // TODO this option is not supported in the code. } output_type_t; + typedef enum : uint8_t + { + PWM =0, + DSHOT=1, + } pwm_protocol_t; + + typedef struct { output_type_t output_type[NUM_MIXER_OUTPUTS]; +// pwm_protocol_t pwm_protocol[NUM_MIXER_OUTPUTS]; // Note: Let the rate determine the protocol PWM vs. DSHOT + float default_pwm_rate[NUM_MIXER_OUTPUTS]; float F[NUM_MIXER_OUTPUTS]; float x[NUM_MIXER_OUTPUTS]; float y[NUM_MIXER_OUTPUTS]; float z[NUM_MIXER_OUTPUTS]; - uint32_t default_pwm_rate; } mixer_t; typedef struct @@ -107,130 +116,148 @@ class Mixer : public ParamListenerInterface void write_motor(uint8_t index, float value); void write_servo(uint8_t index, float value); - const mixer_t esc_calibration_mixing = {{M, M, M, M, M, M, NONE, NONE}, - - {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f}, // F Mix - {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - 490}; + // clang-format off - const mixer_t quadcopter_plus_mixing = { + const mixer_t esc_calibration_mixing = { // + {M, M, M, M, M, M, NONE, NONE, NONE, NONE}, // +// { PWM, PWM, PWM, PWM, PWM, PWM, PWM, PWM, PWM, PWM}, // PWM or DSHOT + { 50, 50, 50, 50, 50, 50, 50, 50, 50, 50}, // Rate (Hz or kHz) + {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // F Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // X Mix + + const mixer_t quadcopter_plus_mixing = { // + {M, M, M, M, NONE, NONE, NONE, NONE, NONE, NONE}, // output_type +// { DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT}, // PWM or DSHOT + {3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5}, // Rate (Hz) + { 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix + { 0.0f, -1.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + { 1.0f, 0.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + { 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix + + const mixer_t quadcopter_x_mixing = { // {M, M, M, M, NONE, NONE, NONE, NONE}, // output_type - - {1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix - {0.0f, -1.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - {1.0f, 0.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix - {1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Z Mix - 490}; - - const mixer_t quadcopter_x_mixing = {{M, M, M, M, NONE, NONE, NONE, NONE}, // output_type - - {1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix - {-1.0f, -1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - {1.0f, -1.0f, -1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix - {1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Z Mix - 490}; - - const mixer_t hex_plus_mixing = { - {M, M, M, M, M, M, M, M}, // output_type - - {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // F Mix - {0.0f, -0.866025f, -0.866025f, 0.0f, 0.866025f, 0.866025f, 0.0f, 0.0f}, // X Mix - {1.0f, 0.5f, -0.5f, -1.0f, -0.5f, 0.5f, 0.0f, 0.0f}, // Y Mix - {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f}, // Z Mix - 490}; - - const mixer_t hex_x_mixing = { - {M, M, M, M, M, M, M, M}, // output_type - - {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // F Mix - {-0.5f, -1.0f, -0.5f, 0.5f, 1.0f, 0.5f, 0.0f, 0.0f}, // X Mix - {0.866025f, 0.0f, -0.866025f, -0.866025f, 0.0f, 0.866025f, 0.0f, 0.0f}, // Y Mix - {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f}, // Z Mix - 490}; - - const mixer_t octocopter_plus_mixing = { - {M, M, M, M, M, M, M, M}, // output_type - - {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f}, // F Mix - {0.0f, -0.707f, -1.0f, -0.707f, 0.0f, 0.707f, 1.0f, 0.707f}, // X Mix - {1.0f, 0.707f, 0.0f, -0.707f, -1.0f, -0.707f, 0.0f, 0.707f}, // Y Mix - {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f}, // Z Mix - 490}; - - const mixer_t octocopter_x_mixing = { - {M, M, M, M, M, M, M, M}, // output_type - - {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f}, // F Mix - {-0.414f, -1.0f, -1.0f, -0.414f, 0.414f, 1.0f, 1.0f, 0.414}, // X Mix - {1.0f, 0.414f, -0.414f, -1.0f, -1.0f, -0.414f, 0.414f, 1.0}, // Y Mix - {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f}, // Z Mix - 490}; - - const mixer_t Y6_mixing = { - {M, M, M, M, M, M, NONE, NONE}, // output_type - - {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // F Mix - {-1.0f, -1.0f, 0.0f, 0.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // X Mix - {0.667f, 0.667f, -1.333f, -1.333f, 0.667f, 0.667f, 0.0f, 0.0f}, // Y Mix - {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f}, // Z Mix - 490}; - - const mixer_t X8_mixing = {{M, M, M, M, M, M, M, M}, // output_type - - {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f}, // F Mix - {-1.0f, -1.0f, -1.0f, -1.0f, 1.0f, 1.0f, 1.0f, 1.0f}, // X Mix - {1.0f, 1.0f, -1.0f, -1.0f, -1.0f, -1.0f, 1.0f, 1.0f}, // Y Mix - {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f}, // Z Mix - 490}; +// { DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT}, // PWM or DSHOT + {3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5}, // Rate (Hz) + {1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix + {-1.0f, -1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {1.0f, -1.0f, -1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + {1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix + + const mixer_t hex_plus_mixing = { // + {M, M, M, M, M, M, M, M, NONE, NONE}, // output_type +// { DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT}, // PWM or DSHOT + {3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5}, // Rate (Hz) + {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix + {0.0f, -0.866025f, -0.866025f, 0.0f, 0.866025f, 0.866025f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {1.0f, 0.5f, -0.5f, -1.0f, -0.5f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix + + const mixer_t hex_x_mixing = { // + {M, M, M, M, M, M, M, M, NONE, NONE}, // output_type +// { DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT}, // PWM or DSHOT + {3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5}, // Rate (Hz) + {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix + {-0.5f, -1.0f, -0.5f, 0.5f, 1.0f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {0.866025f, 0.0f, -0.866025f, -0.866025f, 0.0f, 0.866025f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f} }; // Z Mix + + const mixer_t octocopter_plus_mixing = { // + {M, M, M, M, M, M, M, M, NONE, NONE}, // output_type +// { DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT}, // PWM or DSHOT + {3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5}, // Rate (Hz) + {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // F Mix + {0.0f, -0.707f, -1.0f, -0.707f, 0.0f, 0.707f, 1.0f, 0.707f, 0.0f, 0.0f}, // X Mix + {1.0f, 0.707f, 0.0f, -0.707f, -1.0f, -0.707f, 0.0f, 0.707f, 0.0f, 0.0f}, // Y Mix + {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f}}; // Z Mix + + const mixer_t octocopter_x_mixing = { // + {M, M, M, M, M, M, M, M, NONE, NONE}, // output_type +// { DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT}, // PWM or DSHOT + {3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5}, // Rate (Hz) + {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // F Mix + {-0.414f, -1.0f, -1.0f, -0.414f, 0.414f, 1.0f, 1.0f, 0.414, 0.0f, 0.0f}, // X Mix + {1.0f, 0.414f, -0.414f, -1.0f, -1.0f, -0.414f, 0.414f, 1.0, 0.0f, 0.0f}, // Y Mix + {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f}}; // Z Mix + + const mixer_t Y6_mixing = { // + {M, M, M, M, M, M, NONE, NONE, NONE, NONE}, // output_type +// { DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT}, // PWM or DSHOT + {3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5}, // Rate (Hz) + {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix + {-1.0f, -1.0f, 0.0f, 0.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {0.667f, 0.667f, -1.333f, -1.333f, 0.667f, 0.667f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix + + const mixer_t X8_mixing = { // + {M, M, M, M, M, M, M, M, NONE, NONE}, // output_type +// { DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT, DSHOT}, // PWM or DSHOT + {3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5}, // Rate (Hz) + {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // F Mix + {-1.0f, -1.0f, -1.0f, -1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // X Mix + {1.0f, 1.0f, -1.0f, -1.0f, -1.0f, -1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // Y Mix + {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f} };// Z Mix const mixer_t tricopter_mixing = { - {M, M, M, S, NONE, NONE, NONE, NONE}, // output_type - - {1.0f, 0.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix - {-1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - {0.667f, 0.0f, 0.667f, -1.333f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix - {0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Z Mix - 490}; - - const mixer_t fixedwing_mixing = {{S, S, M, S, S, M, NONE, NONE}, // output type - - {0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix - {1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - {0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix - {0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Z Mix - 50}; - - const mixer_t passthrough_mixing = {{NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE}, - - {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix - {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix - {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Z Mix - 50}; + { M, M, M, NONE, S, NONE, NONE, NONE, NONE, NONE}, // output_type +// { DSHOT, DSHOT, DSHOT, DSHOT, PWM, PWM, PWM, PWM, PWM, PWM}, // PWM or DSHOT + { 3e5, 3e5, 3e5, 3e5, 3e5, 50, 50, 50, 50, 50}, // Rate (Hz or kHz) + { 1.000f, 0.000f, 1.000f, 0.000f, 1.000f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix + {-1.000f, 0.000f, 0.000f, 0.000f, 1.000f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + { 0.667f, 0.000f, 0.667f, 0.000f, -1.333f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + { 0.000f, 1.000f, 0.000f, 0.000f, 0.000f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f} }; // Z Mix + + const mixer_t fixedwing_mixing = { + { S, S, S, NONE, M, NONE, NONE, NONE, NONE, NONE}, // output type +// { PWM, PWM, PWM, PWM, PWM, PWM, PWM, PWM, PWM, PWM}, // PWM or DSHOT + { 50, 50, 50, 50, 50, 50, 50, 50, 50, 50}, // Rate (Hz or kHz) + {0.0f, 0.0f, 1.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix + {1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + {0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix const mixer_t fixedwing_vtail_mixing = { - {S, S, M, S, NONE, NONE, NONE, NONE}, // Ailerons, LRuddervator, Motor, RRuddervator - - {0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix - {1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - {0.0f, -0.5f, 0.0f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix - {0.0f, 0.5f, 0.0f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f}, // Z Mix - 50}; - - const mixer_t custom_mixing = {{NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE}, // output type - - {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix - {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix - {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Z Mix - 50}; + { S, S, S, NONE, M, NONE, NONE, NONE, NONE, NONE}, // Ailerons, LRuddervator, RRuddervator, Motor +// { PWM, PWM, PWM, PWM, PWM, PWM, PWM, PWM, PWM, PWM}, // PWM or DSHOT + { 50, 50, 50, 50, 50, 50, 50, 50, 50, 50}, // Rate (Hz or kHz) + {0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix + {1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {0.0f, -0.5f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + {0.0f, 0.5f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix + + const mixer_t quadplane_mixing = { + { S, S, S, M, M, M, M, M, NONE, NONE}, // Ailerons, Rudder, Elevator, Tractor Motor, Quadrotors + { PWM, PWM, PWM, PWM, DSHOT, DSHOT, DSHOT, DSHOT, PWM, PWM}, // PWM or DSHOT +// { 50, 50, 50, 50, 3e5, 3e5, 3e5, 3e5, 50, 50}, // Rate (Hz or kHz) + {0.0f, 0.0f, 0.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // F Mix + {1.0f, 0.0f, 0.0f, 0.0f, 0.0f,-1.0f, 0.0f, 1.0f, 0.0f, 0.0f}, // X Mix + {0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f,-1.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + {0.0f, 0.0f, 1.0f, 0.0f, 1.0f,-1.0f, 1.0f,-1.0f, 0.0f, 0.0f}}; // Z Mix + + const mixer_t passthrough_mixing = { // + {NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE}, +// { PWM, PWM, PWM, PWM, PWM, PWM, PWM, PWM, PWM, PWM}, // PWM or DSHOT + { 50, 50, 50, 50, 50, 50, 50, 50, 50, 50}, // Rate (Hz or kHz) + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix + + const mixer_t custom_mixing = { // + {NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE}, // output type +// { PWM, PWM, PWM, PWM, PWM, PWM, PWM, PWM, PWM, PWM}, // PWM or DSHOT + { 50, 50, 50, 50, 50, 50, 50, 50, 50, 50}, // Rate (Hz or kHz) + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix const mixer_t * mixer_to_use_; - // clang-format off - const mixer_t* array_of_mixers_[NUM_MIXERS] = {&esc_calibration_mixing, + const mixer_t* array_of_mixers_[NUM_MIXERS] = + { + &esc_calibration_mixing, &quadcopter_plus_mixing, &quadcopter_x_mixing, &hex_plus_mixing, @@ -243,7 +270,9 @@ class Mixer : public ParamListenerInterface &fixedwing_mixing, &passthrough_mixing, &fixedwing_vtail_mixing, - &custom_mixing}; + &quadplane_mixing, + &custom_mixing + }; // clang-format on public: diff --git a/src/comm_manager.cpp b/src/comm_manager.cpp deleted file mode 100644 index de28bbd7..00000000 --- a/src/comm_manager.cpp +++ /dev/null @@ -1,608 +0,0 @@ -/* - * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ - -#include "comm_manager.h" - -#include "param.h" - -#include "rosflight.h" - -#include -#include - -namespace rosflight_firmware -{ -CommManager::LogMessageBuffer::LogMessageBuffer() { memset(buffer_, 0, sizeof(buffer_)); } - -void CommManager::LogMessageBuffer::add_message(CommLinkInterface::LogSeverity severity, char msg[]) -{ - LogMessage & newest_msg = buffer_[newest_]; - strcpy(newest_msg.msg, msg); - newest_msg.severity = severity; - - newest_ = (newest_ + 1) % LOG_BUF_SIZE; - - // quietly over-write old messages (what else can we do?) - length_ += 1; - if (length_ > LOG_BUF_SIZE) { - length_ = LOG_BUF_SIZE; - oldest_ = (oldest_ + 1) % LOG_BUF_SIZE; - } -} - -void CommManager::LogMessageBuffer::pop() -{ - if (length_ > 0) { - length_--; - oldest_ = (oldest_ + 1) % LOG_BUF_SIZE; - } -} - -CommManager::CommManager(ROSflight & rf, CommLinkInterface & comm_link) - : RF_(rf) - , comm_link_(comm_link) -{} - -// function definitions -void CommManager::init() -{ - comm_link_.init(static_cast(RF_.params_.get_param_int(PARAM_BAUD_RATE)), - static_cast(RF_.params_.get_param_int(PARAM_SERIAL_DEVICE))); - - offboard_control_time_ = 0; - send_params_index_ = PARAMS_COUNT; - - update_system_id(PARAM_SYSTEM_ID); - - initialized_ = true; -} - -void CommManager::param_change_callback(uint16_t param_id) -{ - switch (param_id) { - case PARAM_SYSTEM_ID: - update_system_id(param_id); - break; - default: - // do nothing - break; - } -} - -void CommManager::update_system_id(uint16_t param_id) -{ - sysid_ = static_cast(RF_.params_.get_param_int(param_id)); -} - -void CommManager::update_status() { send_status(); } - -void CommManager::send_param_value(uint16_t param_id) -{ - if (param_id < PARAMS_COUNT) { - switch (RF_.params_.get_param_type(param_id)) { - case PARAM_TYPE_INT32: - comm_link_.send_param_value_int(sysid_, param_id, RF_.params_.get_param_name(param_id), - RF_.params_.get_param_int(param_id), - static_cast(PARAMS_COUNT)); - break; - case PARAM_TYPE_FLOAT: - comm_link_.send_param_value_float(sysid_, param_id, RF_.params_.get_param_name(param_id), - RF_.params_.get_param_float(param_id), - static_cast(PARAMS_COUNT)); - break; - default: - break; - } - } -} - -void CommManager::param_request_list_callback(uint8_t target_system) -{ - if (target_system == sysid_) send_params_index_ = 0; -} - -void CommManager::send_parameter_list() { send_params_index_ = 0; } - -void CommManager::param_request_read_callback(uint8_t target_system, const char * const param_name, - int16_t param_index) -{ - if (target_system == sysid_) { - uint16_t id = (param_index < 0) ? RF_.params_.lookup_param_id(param_name) - : static_cast(param_index); - - if (id < PARAMS_COUNT) send_param_value(id); - } -} - -void CommManager::param_set_int_callback(uint8_t target_system, const char * const param_name, - int32_t param_value) -{ - if (target_system == sysid_) { - uint16_t id = RF_.params_.lookup_param_id(param_name); - - if (id < PARAMS_COUNT && RF_.params_.get_param_type(id) == PARAM_TYPE_INT32) { - RF_.params_.set_param_int(id, param_value); - } - } -} - -void CommManager::param_set_float_callback(uint8_t target_system, const char * const param_name, - float param_value) -{ - if (target_system == sysid_) { - uint16_t id = RF_.params_.lookup_param_id(param_name); - - if (id < PARAMS_COUNT && RF_.params_.get_param_type(id) == PARAM_TYPE_FLOAT) { - RF_.params_.set_param_float(id, param_value); - } - } -} - -void CommManager::command_callback(CommLinkInterface::Command command) -{ - bool result; - bool reboot_flag = false; - bool reboot_to_bootloader_flag = false; - - // None of these actions can be performed if we are armed - if (RF_.state_manager_.state().armed) { - result = false; - } else { - result = true; - switch (command) { - case CommLinkInterface::Command::COMMAND_READ_PARAMS: - result = RF_.params_.read(); - break; - case CommLinkInterface::Command::COMMAND_WRITE_PARAMS: - result = RF_.params_.write(); - break; - case CommLinkInterface::Command::COMMAND_SET_PARAM_DEFAULTS: - RF_.params_.set_defaults(); - break; - case CommLinkInterface::Command::COMMAND_ACCEL_CALIBRATION: - result = RF_.sensors_.start_imu_calibration(); - break; - case CommLinkInterface::Command::COMMAND_GYRO_CALIBRATION: - result = RF_.sensors_.start_gyro_calibration(); - break; - case CommLinkInterface::Command::COMMAND_BARO_CALIBRATION: - result = RF_.sensors_.start_baro_calibration(); - break; - case CommLinkInterface::Command::COMMAND_AIRSPEED_CALIBRATION: - result = RF_.sensors_.start_diff_pressure_calibration(); - break; - case CommLinkInterface::Command::COMMAND_RC_CALIBRATION: - RF_.controller_.calculate_equilbrium_torque_from_rc(); - break; - case CommLinkInterface::Command::COMMAND_REBOOT: - reboot_flag = true; - break; - case CommLinkInterface::Command::COMMAND_REBOOT_TO_BOOTLOADER: - reboot_to_bootloader_flag = true; - break; - case CommLinkInterface::Command::COMMAND_SEND_VERSION: - comm_link_.send_version(sysid_, GIT_VERSION_STRING); - break; - } - } - - comm_link_.send_command_ack(sysid_, command, result); - - if (reboot_flag || reboot_to_bootloader_flag) { - RF_.board_.clock_delay(20); - RF_.board_.board_reset(reboot_to_bootloader_flag); - } - RF_.board_.serial_flush(); -} - -void CommManager::timesync_callback(int64_t tc1, int64_t ts1) -{ - uint64_t now_us = RF_.board_.clock_micros(); - - if (tc1 == 0) // check that this is a request, not a response - comm_link_.send_timesync(sysid_, static_cast(now_us) * 1000, ts1); -} - -void CommManager::offboard_control_callback(const CommLinkInterface::OffboardControl & control) -{ - // put values into a new command struct - control_t new_offboard_command; - new_offboard_command.x.value = control.x.value; - new_offboard_command.y.value = control.y.value; - new_offboard_command.z.value = control.z.value; - new_offboard_command.F.value = control.F.value; - - // Move flags into standard message - new_offboard_command.x.active = control.x.valid; - new_offboard_command.y.active = control.y.valid; - new_offboard_command.z.active = control.z.valid; - new_offboard_command.F.active = control.F.valid; - - // translate modes into standard message - switch (control.mode) { - case CommLinkInterface::OffboardControl::Mode::PASS_THROUGH: - new_offboard_command.x.type = PASSTHROUGH; - new_offboard_command.y.type = PASSTHROUGH; - new_offboard_command.z.type = PASSTHROUGH; - new_offboard_command.F.type = THROTTLE; - break; - case CommLinkInterface::OffboardControl::Mode::ROLLRATE_PITCHRATE_YAWRATE_THROTTLE: - new_offboard_command.x.type = RATE; - new_offboard_command.y.type = RATE; - new_offboard_command.z.type = RATE; - new_offboard_command.F.type = THROTTLE; - break; - case CommLinkInterface::OffboardControl::Mode::ROLL_PITCH_YAWRATE_THROTTLE: - new_offboard_command.x.type = ANGLE; - new_offboard_command.y.type = ANGLE; - new_offboard_command.z.type = RATE; - new_offboard_command.F.type = THROTTLE; - break; - } - - // Tell the command_manager that we have a new command we need to mux - new_offboard_command.stamp_ms = RF_.board_.clock_millis(); - RF_.command_manager_.set_new_offboard_command(new_offboard_command); -} - -void CommManager::aux_command_callback(const CommLinkInterface::AuxCommand & command) -{ - Mixer::aux_command_t new_aux_command; - - for (int i = 0; i < 14; i++) { - switch (command.cmd_array[i].type) { - case CommLinkInterface::AuxCommand::Type::DISABLED: - // Channel is either not used or is controlled by the mixer - new_aux_command.channel[i].type = Mixer::NONE; - new_aux_command.channel[i].value = 0; - break; - case CommLinkInterface::AuxCommand::Type::SERVO: - // PWM value should be mapped to servo position - new_aux_command.channel[i].type = Mixer::S; - new_aux_command.channel[i].value = command.cmd_array[i].value; - break; - case CommLinkInterface::AuxCommand::Type::MOTOR: - // PWM value should be mapped to motor speed - new_aux_command.channel[i].type = Mixer::M; - new_aux_command.channel[i].value = command.cmd_array[i].value; - break; - } - } - - // Send the new aux_command to the mixer - RF_.mixer_.set_new_aux_command(new_aux_command); -} - -void CommManager::external_attitude_callback(const turbomath::Quaternion & q) -{ - RF_.estimator_.set_external_attitude_update(q); -} - -void CommManager::heartbeat_callback(void) -{ - // receiving a heartbeat implies that a connection has been made - // to the off-board computer. - connected_ = true; - - // send backup data if we have it buffered - if (have_backup_data_) { - comm_link_.send_error_data(sysid_, backup_data_buffer_); - have_backup_data_ = false; - } -} - -// function definitions -void CommManager::receive(void) { comm_link_.receive(); } - -void CommManager::log(CommLinkInterface::LogSeverity severity, const char * fmt, ...) -{ - // Convert the format string to a raw char array - va_list args; - va_start(args, fmt); - char text[LOG_MSG_SIZE]; - vsnprintf(text, LOG_MSG_SIZE, fmt, args); - va_end(args); - - if (initialized_ && connected_) { - comm_link_.send_log_message(sysid_, severity, text); - } else { - log_buffer_.add_message(severity, text); - } -} - -void CommManager::send_heartbeat(void) -{ - comm_link_.send_heartbeat(sysid_, static_cast(RF_.params_.get_param_int(PARAM_FIXED_WING))); -} - -void CommManager::send_status(void) -{ - if (!initialized_) return; - - uint8_t control_mode = 0; - if (RF_.params_.get_param_int(PARAM_FIXED_WING)) control_mode = MODE_PASS_THROUGH; - else if (RF_.command_manager_.combined_control().x.type == ANGLE) - control_mode = MODE_ROLL_PITCH_YAWRATE_THROTTLE; - else - control_mode = MODE_ROLLRATE_PITCHRATE_YAWRATE_THROTTLE; - - comm_link_.send_status( - sysid_, RF_.state_manager_.state().armed, RF_.state_manager_.state().failsafe, - RF_.command_manager_.rc_override_active(), RF_.command_manager_.offboard_control_active(), - RF_.state_manager_.state().error_codes, control_mode, RF_.board_.num_sensor_errors(), - RF_.get_loop_time_us()); -} - -void CommManager::send_attitude(void) -{ - comm_link_.send_attitude_quaternion(sysid_, RF_.estimator_.state().timestamp_us, - RF_.estimator_.state().attitude, - RF_.estimator_.state().angular_velocity); -} - -void CommManager::send_imu(void) -{ - turbomath::Vector acc, gyro; - uint64_t stamp_us; - RF_.sensors_.get_filtered_IMU(acc, gyro, stamp_us); - comm_link_.send_imu(sysid_, stamp_us, acc, gyro, RF_.sensors_.data().imu_temperature); -} - -void CommManager::send_output_raw(void) -{ - comm_link_.send_output_raw(sysid_, RF_.board_.clock_millis(), RF_.mixer_.get_outputs()); -} - -void CommManager::send_rc_raw(void) -{ - // TODO better mechanism for retreiving RC (through RC module, not PWM-specific) - uint16_t channels[8] = {static_cast(RF_.board_.rc_read(0) * 1000 + 1000), - static_cast(RF_.board_.rc_read(1) * 1000 + 1000), - static_cast(RF_.board_.rc_read(2) * 1000 + 1000), - static_cast(RF_.board_.rc_read(3) * 1000 + 1000), - static_cast(RF_.board_.rc_read(4) * 1000 + 1000), - static_cast(RF_.board_.rc_read(5) * 1000 + 1000), - static_cast(RF_.board_.rc_read(6) * 1000 + 1000), - static_cast(RF_.board_.rc_read(7) * 1000 + 1000)}; - comm_link_.send_rc_raw(sysid_, RF_.board_.clock_millis(), channels); -} - -void CommManager::send_diff_pressure(void) -{ - if (RF_.sensors_.data().diff_pressure_valid) { - comm_link_.send_diff_pressure(sysid_, RF_.sensors_.data().diff_pressure_velocity, - RF_.sensors_.data().diff_pressure, - RF_.sensors_.data().diff_pressure_temp); - } -} - -void CommManager::send_baro(void) -{ - if (RF_.sensors_.data().baro_valid) { - comm_link_.send_baro(sysid_, RF_.sensors_.data().baro_altitude, - RF_.sensors_.data().baro_pressure, RF_.sensors_.data().baro_temperature); - } -} - -void CommManager::send_sonar(void) -{ - if (RF_.sensors_.data().sonar_range_valid) { - comm_link_.send_sonar(sysid_, - 0, // TODO set sensor type (sonar/lidar), use enum - RF_.sensors_.data().sonar_range, 8.0, 0.25); - } -} - -void CommManager::send_mag(void) -{ - if (RF_.sensors_.data().mag_present) comm_link_.send_mag(sysid_, RF_.sensors_.data().mag); -} -void CommManager::send_battery_status(void) -{ - if (RF_.sensors_.data().battery_monitor_present) - comm_link_.send_battery_status(sysid_, RF_.sensors_.data().battery_voltage, - RF_.sensors_.data().battery_current); -} - -void CommManager::send_backup_data(const StateManager::BackupData & backup_data) -{ - if (connected_) { - comm_link_.send_error_data(sysid_, backup_data); - } else { - backup_data_buffer_ = backup_data; - have_backup_data_ = true; - } -} - -void CommManager::send_gnss(void) -{ - const GNSSData & gnss_data = RF_.sensors_.data().gnss_data; - - if (RF_.sensors_.data().gnss_present) { - if (gnss_data.time_of_week != last_sent_gnss_tow_) { - comm_link_.send_gnss(sysid_, gnss_data); - last_sent_gnss_tow_ = gnss_data.time_of_week; - } - } -} - -void CommManager::send_gnss_full() -{ - const GNSSFull & gnss_full = RF_.sensors_.data().gnss_full; - - if (RF_.sensors_.data().gnss_present) { - if (gnss_full.time_of_week != last_sent_gnss_full_tow_) { - comm_link_.send_gnss_full(sysid_, RF_.sensors_.data().gnss_full); - last_sent_gnss_full_tow_ = gnss_full.time_of_week; - } - } -} - -void CommManager::send_low_priority(void) -{ - send_next_param(); - - // send buffered log messages - if (connected_ && !log_buffer_.empty()) { - const LogMessageBuffer::LogMessage & msg = log_buffer_.oldest(); - comm_link_.send_log_message(sysid_, msg.severity, msg.msg); - log_buffer_.pop(); - } -} - -// function definitions -void CommManager::stream(got_flags got) -{ - uint64_t time_us = RF_.board_.clock_micros(); - - // Send out data - - if (got.imu) // Nominally 400Hz - { - send_imu(); - send_attitude(); - static uint64_t ro_count = 0; - if (!((ro_count++) % 8)) send_output_raw(); // Raw output at 400Hz/8 = 50Hz - } - - // Pitot sensor - if (got.diff_pressure) send_diff_pressure(); - // Baro altitude - if (got.baro) send_baro(); - // Magnetometer - if (got.mag) send_mag(); - // Height above ground sensor (not enabled) - if (got.sonar) send_sonar(); - // Battery V & I - if (got.battery) send_battery_status(); - // GPS data (GNSS Packed) - if (got.gnss) send_gnss(); - // GPS full data (not needed) - if (got.gnss_full) send_gnss_full(); - if (got.rc) send_rc_raw(); - - { - static uint64_t next_heartbeat = 0, next_status = 0; - - if ((time_us) / 1000000 >= next_heartbeat) // 1 Hz - { - send_heartbeat(); - next_heartbeat = time_us / 1000000 + 1; - } - if ((time_us) / 100000 >= next_status) // 10 Hz - { - send_status(); - next_status = time_us / 100000 + 1; - } - } - - send_low_priority(); // parameter values and logging messages -} - -void CommManager::send_named_value_int(const char * const name, int32_t value) -{ - comm_link_.send_named_value_int(sysid_, RF_.board_.clock_millis(), name, value); -} - -void CommManager::send_named_value_float(const char * const name, float value) -{ - comm_link_.send_named_value_float(sysid_, RF_.board_.clock_millis(), name, value); -} - -void CommManager::send_next_param(void) -{ - if (send_params_index_ < PARAMS_COUNT) { - send_param_value(static_cast(send_params_index_)); - send_params_index_++; - } -} - -CommManager::Stream::Stream(uint32_t period_us, std::function send_function) - : period_us_(period_us) - , next_time_us_(0) - , send_function_(send_function) -{} - -void CommManager::Stream::stream(uint64_t now_us) -{ - if (period_us_ > 0 && now_us >= next_time_us_) { - // if you fall behind, skip messages - do { - next_time_us_ += period_us_; - } while (next_time_us_ < now_us); - - send_function_(); - } -} - -void CommManager::Stream::set_rate(uint32_t rate_hz) -{ - period_us_ = (rate_hz == 0) ? 0 : 1000000 / rate_hz; -} - -// void Mavlink::mavlink_send_named_command_struct(const char *const name, control_t command_struct) -//{ -// uint8_t control_mode; -// if (command_struct.x.type == RATE && command_struct.y.type == RATE) -// { -// control_mode = MODE_ROLLRATE_PITCHRATE_YAWRATE_THROTTLE; -// } -// else if (command_struct.x.type == ANGLE && command_struct.y.type == ANGLE) -// { -// if (command_struct.x.type == ALTITUDE) -// { -// control_mode = MODE_ROLL_PITCH_YAWRATE_ALTITUDE; -// } -// else -// { -// control_mode = MODE_ROLL_PITCH_YAWRATE_THROTTLE; -// } -// } -// else -// { -// control_mode = MODE_PASS_THROUGH; -// } -// uint8_t ignore = !(command_struct.x.active) || -// !(command_struct.y.active) << 1 || -// !(command_struct.z.active) << 2 || -// !(command_struct.F.active) << 3; -// mavlink_message_t msg; -// mavlink_msg_named_command_struct_pack(sysid, compid, &msg, name, -// control_mode, -// ignore, -// command_struct.x.value, -// command_struct.y.value, -// command_struct.z.value, -// command_struct.F.value); -// send_message(msg); -//} - -} // namespace rosflight_firmware diff --git a/src/command_manager.cpp b/src/command_manager.cpp deleted file mode 100644 index abab3aff..00000000 --- a/src/command_manager.cpp +++ /dev/null @@ -1,285 +0,0 @@ -/* - * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ - -#include "command_manager.h" - -#include "rosflight.h" - -#include -#include - -namespace rosflight_firmware -{ -typedef enum -{ - ATT_MODE_RATE, - ATT_MODE_ANGLE -} att_mode_t; - -typedef struct -{ - RC::Stick rc_channel; - uint32_t last_override_time; -} rc_stick_override_t; - -rc_stick_override_t rc_stick_override[] = {{RC::STICK_X, 0}, {RC::STICK_Y, 0}, {RC::STICK_Z, 0}}; - -typedef struct -{ - control_channel_t * rc; - control_channel_t * onboard; - control_channel_t * combined; -} mux_t; - -CommandManager::CommandManager(ROSflight & _rf) - : RF_(_rf) - , failsafe_command_(multirotor_failsafe_command_) -{} - -void CommandManager::init() { init_failsafe(); } - -void CommandManager::param_change_callback(uint16_t param_id) -{ - switch (param_id) { - case PARAM_FIXED_WING: - case PARAM_FAILSAFE_THROTTLE: - init_failsafe(); - break; - default: - // do nothing - break; - } -} - -void CommandManager::init_failsafe() -{ - float failsafe_thr_param = RF_.params_.get_param_float(PARAM_FAILSAFE_THROTTLE); - bool fixedwing = RF_.params_.get_param_int(PARAM_FIXED_WING); - if (!fixedwing - && (failsafe_thr_param < 0. - || failsafe_thr_param > 1.0)) // fixed wings always have 0 failsafe throttle - { - RF_.state_manager_.set_error(StateManager::ERROR_INVALID_FAILSAFE); - failsafe_thr_param = 0.; - } else { - RF_.state_manager_.clear_error(StateManager::ERROR_INVALID_FAILSAFE); - } - - multirotor_failsafe_command_.F.value = failsafe_thr_param; - - if (fixedwing) failsafe_command_ = fixedwing_failsafe_command_; - else - failsafe_command_ = multirotor_failsafe_command_; -} - -void CommandManager::interpret_rc(void) -{ - // get initial, unscaled RC values - rc_command_.x.value = RF_.rc_.stick(RC::STICK_X); - rc_command_.y.value = RF_.rc_.stick(RC::STICK_Y); - rc_command_.z.value = RF_.rc_.stick(RC::STICK_Z); - rc_command_.F.value = RF_.rc_.stick(RC::STICK_F); - - // determine control mode for each channel and scale command values accordingly - if (RF_.params_.get_param_int(PARAM_FIXED_WING)) { - rc_command_.x.type = PASSTHROUGH; - rc_command_.y.type = PASSTHROUGH; - rc_command_.z.type = PASSTHROUGH; - rc_command_.F.type = THROTTLE; - } else { - // roll and pitch - control_type_t roll_pitch_type; - if (RF_.rc_.switch_mapped(RC::SWITCH_ATT_TYPE)) { - roll_pitch_type = RF_.rc_.switch_on(RC::SWITCH_ATT_TYPE) ? ANGLE : RATE; - } else { - roll_pitch_type = - (RF_.params_.get_param_int(PARAM_RC_ATTITUDE_MODE) == ATT_MODE_RATE) ? RATE : ANGLE; - } - - rc_command_.x.type = roll_pitch_type; - rc_command_.y.type = roll_pitch_type; - - // Scale command to appropriate units - switch (roll_pitch_type) { - case RATE: - rc_command_.x.value *= RF_.params_.get_param_float(PARAM_RC_MAX_ROLLRATE); - rc_command_.y.value *= RF_.params_.get_param_float(PARAM_RC_MAX_PITCHRATE); - break; - case ANGLE: - rc_command_.x.value *= RF_.params_.get_param_float(PARAM_RC_MAX_ROLL); - rc_command_.y.value *= RF_.params_.get_param_float(PARAM_RC_MAX_PITCH); - default: - break; - } - - // yaw - rc_command_.z.type = RATE; - rc_command_.z.value *= RF_.params_.get_param_float(PARAM_RC_MAX_YAWRATE); - - // throttle - rc_command_.F.type = THROTTLE; - } -} - -bool CommandManager::stick_deviated(MuxChannel channel) -{ - uint32_t now = RF_.board_.clock_millis(); - - // if we are still in the lag time, return true - if (now < rc_stick_override_[channel].last_override_time - + RF_.params_.get_param_int(PARAM_OVERRIDE_LAG_TIME)) { - return true; - } else { - if (fabsf(RF_.rc_.stick(rc_stick_override_[channel].rc_channel)) - > RF_.params_.get_param_float(PARAM_RC_OVERRIDE_DEVIATION)) { - rc_stick_override_[channel].last_override_time = now; - return true; - } - return false; - } -} - -bool CommandManager::do_roll_pitch_yaw_muxing(MuxChannel channel) -{ - bool override_this_channel = false; - // Check if the override switch exists and is triggered, or if the sticks have deviated enough to - // trigger an override - if ((RF_.rc_.switch_mapped(RC::SWITCH_ATT_OVERRIDE) && RF_.rc_.switch_on(RC::SWITCH_ATT_OVERRIDE)) - || stick_deviated(channel)) { - override_this_channel = true; - } else // Otherwise only have RC override if the offboard channel is inactive - { - if (muxes[channel].onboard->active) { - override_this_channel = false; - } else { - override_this_channel = true; - } - } - // set the combined channel output depending on whether RC is overriding for this channel or not - *muxes[channel].combined = override_this_channel ? *muxes[channel].rc : *muxes[channel].onboard; - return override_this_channel; -} - -bool CommandManager::do_throttle_muxing(void) -{ - bool override_this_channel = false; - // Check if the override switch exists and is triggered - if (RF_.rc_.switch_mapped(RC::SWITCH_THROTTLE_OVERRIDE) - && RF_.rc_.switch_on(RC::SWITCH_THROTTLE_OVERRIDE)) { - override_this_channel = true; - } else // Otherwise check if the offboard throttle channel is active, if it isn't, have RC override - { - if (muxes[MUX_F].onboard->active) { - // Check if the parameter flag is set to have us always take the smaller throttle - if (RF_.params_.get_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE)) { - override_this_channel = (muxes[MUX_F].rc->value < muxes[MUX_F].onboard->value); - } else { - override_this_channel = false; - } - } else { - override_this_channel = true; - } - } - - // Set the combined channel output depending on whether RC is overriding for this channel or not - *muxes[MUX_F].combined = override_this_channel ? *muxes[MUX_F].rc : *muxes[MUX_F].onboard; - return override_this_channel; -} - -bool CommandManager::rc_override_active() { return rc_override_; } - -bool CommandManager::offboard_control_active() -{ - for (int i = 0; i < 4; i++) { - if (muxes[i].onboard->active) return true; - } - return false; -} - -void CommandManager::set_new_offboard_command(control_t new_offboard_command) -{ - new_command_ = true; - offboard_command_ = new_offboard_command; -} - -void CommandManager::set_new_rc_command(control_t new_rc_command) -{ - new_command_ = true; - rc_command_ = new_rc_command; -} - -void CommandManager::override_combined_command_with_rc() -{ - new_command_ = true; - combined_command_ = rc_command_; -} - -bool CommandManager::run() -{ - bool last_rc_override = rc_override_; - - // Check for and apply failsafe command - if (RF_.state_manager_.state().failsafe) { - combined_command_ = failsafe_command_; - } else if (RF_.rc_.new_command()) { - // Read RC - interpret_rc(); - - // Check for offboard control timeout (100 ms) - if (RF_.board_.clock_millis() - > offboard_command_.stamp_ms + RF_.params_.get_param_int(PARAM_OFFBOARD_TIMEOUT)) { - // If it has been longer than 100 ms, then disable the offboard control - offboard_command_.F.active = false; - offboard_command_.x.active = false; - offboard_command_.y.active = false; - offboard_command_.z.active = false; - } - - // Perform muxing - rc_override_ = do_roll_pitch_yaw_muxing(MUX_X); - rc_override_ |= do_roll_pitch_yaw_muxing(MUX_Y); - rc_override_ |= do_roll_pitch_yaw_muxing(MUX_Z); - rc_override_ |= do_throttle_muxing(); - - // Light to indicate override - if (rc_override_) { - RF_.board_.led0_on(); - } else { - RF_.board_.led0_off(); - } - } - - // There was a change in rc_override state - if (last_rc_override != rc_override_) { RF_.comm_manager_.update_status(); } - return true; -} - -} // namespace rosflight_firmware diff --git a/src/controller.cpp b/src/controller.cpp deleted file mode 100644 index e04036fb..00000000 --- a/src/controller.cpp +++ /dev/null @@ -1,290 +0,0 @@ -/* - * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ - -#include "controller.h" - -#include "command_manager.h" -#include "estimator.h" - -#include "rosflight.h" - -#include -#include - -namespace rosflight_firmware -{ -Controller::Controller(ROSflight & rf) - : RF_(rf) -{} - -void Controller::init() -{ - prev_time_us_ = 0; - - float max = RF_.params_.get_param_float(PARAM_MAX_COMMAND); - float min = -max; - float tau = RF_.params_.get_param_float(PARAM_PID_TAU); - - roll_.init(RF_.params_.get_param_float(PARAM_PID_ROLL_ANGLE_P), - RF_.params_.get_param_float(PARAM_PID_ROLL_ANGLE_I), - RF_.params_.get_param_float(PARAM_PID_ROLL_ANGLE_D), max, min, tau); - roll_rate_.init(RF_.params_.get_param_float(PARAM_PID_ROLL_RATE_P), - RF_.params_.get_param_float(PARAM_PID_ROLL_RATE_I), - RF_.params_.get_param_float(PARAM_PID_ROLL_RATE_D), max, min, tau); - pitch_.init(RF_.params_.get_param_float(PARAM_PID_PITCH_ANGLE_P), - RF_.params_.get_param_float(PARAM_PID_PITCH_ANGLE_I), - RF_.params_.get_param_float(PARAM_PID_PITCH_ANGLE_D), max, min, tau); - pitch_rate_.init(RF_.params_.get_param_float(PARAM_PID_PITCH_RATE_P), - RF_.params_.get_param_float(PARAM_PID_PITCH_RATE_I), - RF_.params_.get_param_float(PARAM_PID_PITCH_RATE_D), max, min, tau); - yaw_rate_.init(RF_.params_.get_param_float(PARAM_PID_YAW_RATE_P), - RF_.params_.get_param_float(PARAM_PID_YAW_RATE_I), - RF_.params_.get_param_float(PARAM_PID_YAW_RATE_D), max, min, tau); -} - -void Controller::run() -{ - // Time calculation - if (prev_time_us_ == 0) { - prev_time_us_ = RF_.estimator_.state().timestamp_us; - return; - } - - int32_t dt_us = (RF_.estimator_.state().timestamp_us - prev_time_us_); - if (dt_us < 0) { - RF_.state_manager_.set_error(StateManager::ERROR_TIME_GOING_BACKWARDS); - return; - } - prev_time_us_ = RF_.estimator_.state().timestamp_us; - - // Check if integrators should be updated - //! @todo better way to figure out if throttle is high - bool update_integrators = (RF_.state_manager_.state().armed) - && (RF_.command_manager_.combined_control().F.value > 0.1f) && dt_us < 10000; - - // Run the PID loops - turbomath::Vector pid_output = run_pid_loops( - dt_us, RF_.estimator_.state(), RF_.command_manager_.combined_control(), update_integrators); - - // Add feedforward torques - output_.x = pid_output.x + RF_.params_.get_param_float(PARAM_X_EQ_TORQUE); - output_.y = pid_output.y + RF_.params_.get_param_float(PARAM_Y_EQ_TORQUE); - output_.z = pid_output.z + RF_.params_.get_param_float(PARAM_Z_EQ_TORQUE); - output_.F = RF_.command_manager_.combined_control().F.value; -} - -void Controller::calculate_equilbrium_torque_from_rc() -{ - // Make sure we are disarmed - if (!(RF_.state_manager_.state().armed)) { - // Tell the user that we are doing a equilibrium torque calibration - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, - "Capturing equilbrium offsets from RC"); - - // Prepare for calibration - // artificially tell the flight controller it is leveled - Estimator::State fake_state; - fake_state.angular_velocity.x = 0.0f; - fake_state.angular_velocity.y = 0.0f; - fake_state.angular_velocity.z = 0.0f; - - fake_state.attitude.x = 0.0f; - fake_state.attitude.y = 0.0f; - fake_state.attitude.z = 0.0f; - fake_state.attitude.w = 1.0f; - - fake_state.roll = 0.0f; - fake_state.pitch = 0.0f; - fake_state.yaw = 0.0f; - - // pass the rc_control through the controller - // dt is zero, so what this really does is applies the P gain with the settings - // your RC transmitter, which if it flies level is a really good guess for - // the static offset torques - turbomath::Vector pid_output = - run_pid_loops(0, fake_state, RF_.command_manager_.rc_control(), false); - - // the output from the controller is going to be the static offsets - RF_.params_.set_param_float(PARAM_X_EQ_TORQUE, - pid_output.x + RF_.params_.get_param_float(PARAM_X_EQ_TORQUE)); - RF_.params_.set_param_float(PARAM_Y_EQ_TORQUE, - pid_output.y + RF_.params_.get_param_float(PARAM_Y_EQ_TORQUE)); - RF_.params_.set_param_float(PARAM_Z_EQ_TORQUE, - pid_output.z + RF_.params_.get_param_float(PARAM_Z_EQ_TORQUE)); - - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, - "Equilibrium torques found and applied."); - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, - "Please zero out trims on your transmitter"); - } else { - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, - "Cannot perform equilibrium offset calibration while armed"); - } -} - -void Controller::param_change_callback(uint16_t param_id) -{ - switch (param_id) { - case PARAM_PID_ROLL_ANGLE_P: - case PARAM_PID_ROLL_ANGLE_I: - case PARAM_PID_ROLL_ANGLE_D: - case PARAM_PID_ROLL_RATE_P: - case PARAM_PID_ROLL_RATE_I: - case PARAM_PID_ROLL_RATE_D: - case PARAM_PID_PITCH_ANGLE_P: - case PARAM_PID_PITCH_ANGLE_I: - case PARAM_PID_PITCH_ANGLE_D: - case PARAM_PID_PITCH_RATE_P: - case PARAM_PID_PITCH_RATE_I: - case PARAM_PID_PITCH_RATE_D: - case PARAM_PID_YAW_RATE_P: - case PARAM_PID_YAW_RATE_I: - case PARAM_PID_YAW_RATE_D: - case PARAM_MAX_COMMAND: - case PARAM_PID_TAU: - init(); - break; - default: - // do nothing - break; - } -} - -turbomath::Vector Controller::run_pid_loops(uint32_t dt_us, const Estimator::State & state, - const control_t & command, bool update_integrators) -{ - // Based on the control types coming from the command manager, run the appropriate PID loops - turbomath::Vector out; - - float dt = 1e-6 * dt_us; - - // ROLL - if (command.x.type == RATE) - out.x = roll_rate_.run(dt, state.angular_velocity.x, command.x.value, update_integrators); - else if (command.x.type == ANGLE) - out.x = - roll_.run(dt, state.roll, command.x.value, update_integrators, state.angular_velocity.x); - else - out.x = command.x.value; - - // PITCH - if (command.y.type == RATE) - out.y = pitch_rate_.run(dt, state.angular_velocity.y, command.y.value, update_integrators); - else if (command.y.type == ANGLE) - out.y = - pitch_.run(dt, state.pitch, command.y.value, update_integrators, state.angular_velocity.y); - else - out.y = command.y.value; - - // YAW - if (command.z.type == RATE) - out.z = yaw_rate_.run(dt, state.angular_velocity.z, command.z.value, update_integrators); - else - out.z = command.z.value; - - return out; -} - -Controller::PID::PID() - : kp_(0.0f) - , ki_(0.0f) - , kd_(0.0f) - , max_(1.0f) - , min_(-1.0f) - , integrator_(0.0f) - , differentiator_(0.0f) - , prev_x_(0.0f) - , tau_(0.05) -{} - -void Controller::PID::init(float kp, float ki, float kd, float max, float min, float tau) -{ - kp_ = kp; - ki_ = ki; - kd_ = kd; - max_ = max; - min_ = min; - tau_ = tau; -} - -float Controller::PID::run(float dt, float x, float x_c, bool update_integrator) -{ - float xdot; - if (dt > 0.0001f) { - // calculate D term (use dirty derivative if we don't have access to a measurement of the - // derivative) The dirty derivative is a sort of low-pass filtered version of the derivative. - //// (Include reference to Dr. Beard's notes here) - differentiator_ = (2.0f * tau_ - dt) / (2.0f * tau_ + dt) * differentiator_ - + 2.0f / (2.0f * tau_ + dt) * (x - prev_x_); - xdot = differentiator_; - } else { - xdot = 0.0f; - } - prev_x_ = x; - - return run(dt, x, x_c, update_integrator, xdot); -} - -float Controller::PID::run(float dt, float x, float x_c, bool update_integrator, float xdot) -{ - // Calculate Error - float error = x_c - x; - - // Initialize Terms - float p_term = error * kp_; - float i_term = 0.0f; - float d_term = 0.0f; - - // If there is a derivative term - if (kd_ > 0.0f) { d_term = kd_ * xdot; } - - // If there is an integrator term and we are updating integrators - if ((ki_ > 0.0f) && update_integrator) { - // integrate - integrator_ += error * dt; - // calculate I term - i_term = ki_ * integrator_; - } - - // sum three terms - float u = p_term - d_term + i_term; - - // Integrator anti-windup - //// Include reference to Dr. Beard's notes here - float u_sat = (u > max_) ? max_ : (u < min_) ? min_ : u; - if (u != u_sat && fabs(i_term) > fabs(u - p_term + d_term) && ki_ > 0.0f) - integrator_ = (u_sat - p_term + d_term) / ki_; - - // Set output - return u_sat; -} - -} // namespace rosflight_firmware diff --git a/src/estimator.cpp b/src/estimator.cpp deleted file mode 100644 index 4db72f0a..00000000 --- a/src/estimator.cpp +++ /dev/null @@ -1,376 +0,0 @@ -/* - * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ - -#include "estimator.h" - -#include "rosflight.h" - -namespace rosflight_firmware -{ -Estimator::Estimator(ROSflight & _rf) - : RF_(_rf) -{} - -void Estimator::reset_state() -{ - state_.attitude.w = 1.0f; - state_.attitude.x = 0.0f; - state_.attitude.y = 0.0f; - state_.attitude.z = 0.0f; - - state_.angular_velocity.x = 0.0f; - state_.angular_velocity.y = 0.0f; - state_.angular_velocity.z = 0.0f; - - state_.roll = 0.0f; - state_.pitch = 0.0f; - state_.yaw = 0.0f; - - w1_.x = 0.0f; - w1_.y = 0.0f; - w1_.z = 0.0f; - - w2_.x = 0.0f; - w2_.y = 0.0f; - w2_.z = 0.0f; - - bias_.x = 0.0f; - bias_.y = 0.0f; - bias_.z = 0.0f; - - accel_LPF_.x = 0; - accel_LPF_.y = 0; - accel_LPF_.z = -9.80665; - - gyro_LPF_.x = 0; - gyro_LPF_.y = 0; - gyro_LPF_.z = 0; - - state_.timestamp_us = RF_.board_.clock_micros(); - - extatt_update_next_run_ = false; - - // Clear the unhealthy estimator flag - RF_.state_manager_.clear_error(StateManager::ERROR_UNHEALTHY_ESTIMATOR); -} - -void Estimator::reset_adaptive_bias() -{ - bias_.x = 0; - bias_.y = 0; - bias_.z = 0; -} - -void Estimator::init() -{ - last_time_ = 0; - last_acc_update_us_ = 0; - last_extatt_update_us_ = 0; - reset_state(); -} - -void Estimator::param_change_callback(uint16_t param_id) { (void) param_id; } - -void Estimator::run_LPF() -{ - float alpha_acc = RF_.params_.get_param_float(PARAM_ACC_ALPHA); - const turbomath::Vector & raw_accel = RF_.sensors_.data().accel; - accel_LPF_.x = (1.0f - alpha_acc) * raw_accel.x + alpha_acc * accel_LPF_.x; - accel_LPF_.y = (1.0f - alpha_acc) * raw_accel.y + alpha_acc * accel_LPF_.y; - accel_LPF_.z = (1.0f - alpha_acc) * raw_accel.z + alpha_acc * accel_LPF_.z; - - float alpha_gyro_xy = RF_.params_.get_param_float(PARAM_GYRO_XY_ALPHA); - float alpha_gyro_z = RF_.params_.get_param_float(PARAM_GYRO_Z_ALPHA); - const turbomath::Vector & raw_gyro = RF_.sensors_.data().gyro; - gyro_LPF_.x = (1.0f - alpha_gyro_xy) * raw_gyro.x + alpha_gyro_xy * gyro_LPF_.x; - gyro_LPF_.y = (1.0f - alpha_gyro_xy) * raw_gyro.y + alpha_gyro_xy * gyro_LPF_.y; - gyro_LPF_.z = (1.0f - alpha_gyro_z) * raw_gyro.z + alpha_gyro_z * gyro_LPF_.z; -} - -void Estimator::set_external_attitude_update(const turbomath::Quaternion & q) -{ - extatt_update_next_run_ = true; - q_extatt_ = q; -} - -void Estimator::run() -{ - // - // Timing Setup - // - - const uint64_t now_us = RF_.sensors_.data().imu_time; - if (last_time_ == 0) { - last_time_ = now_us; - last_acc_update_us_ = now_us; - last_extatt_update_us_ = now_us; - return; - } else if (now_us < last_time_) { - // this shouldn't happen - RF_.state_manager_.set_error(StateManager::ERROR_TIME_GOING_BACKWARDS); - last_time_ = now_us; - return; - } - - RF_.state_manager_.clear_error(StateManager::ERROR_TIME_GOING_BACKWARDS); - - float dt = (now_us - last_time_) * 1e-6f; - last_time_ = now_us; - state_.timestamp_us = now_us; - - // Low-pass filter accel and gyro measurements - run_LPF(); - - // - // Gyro Correction Term (werr) - // - - float kp = 0.0f; - float ki = RF_.params_.get_param_float(PARAM_FILTER_KI); - - turbomath::Vector w_err; - - if (can_use_accel()) { - // Get error estimated by accelerometer measurement - w_err = accel_correction(); - kp = RF_.params_.get_param_float(PARAM_FILTER_KP_ACC); - - last_acc_update_us_ = now_us; - } - - if (can_use_extatt()) { - // Get error estimated by external attitude measurement. Overwrite any - // correction based on the accelerometer (assumption: extatt is better). - w_err = extatt_correction(); - kp = RF_.params_.get_param_float(PARAM_FILTER_KP_EXT); - - // the angular rate correction from external attitude updates occur at a - // different rate than IMU updates, so it needs to be integrated with a - // different dt. The following scales the correction term by the timestep - // ratio so that it is integrated correctly. - const float extAttDt = (now_us - last_extatt_update_us_) * 1e-6f; - const float scaleDt = (dt > 0) ? (extAttDt / dt) : 0.0f; - w_err *= scaleDt; - - last_extatt_update_us_ = now_us; - extatt_update_next_run_ = false; - } - - // Crank up the gains for the first few seconds for quick convergence - if (now_us < static_cast(RF_.params_.get_param_int(PARAM_INIT_TIME)) * 1000) { - kp = RF_.params_.get_param_float(PARAM_FILTER_KP_ACC) * 10.0f; - ki = RF_.params_.get_param_float(PARAM_FILTER_KI) * 10.0f; - } - - // - // Composite Bias-Free Angular Rate (wfinal) - // - - // Integrate biases driven by measured angular error - // eq 47b Mahony Paper, using correction term w_err found above - bias_ -= ki * w_err * dt; - - // Build the composite omega vector for kinematic propagation - // This the stuff inside the p function in eq. 47a - Mahony Paper - turbomath::Vector wbar = smoothed_gyro_measurement(); - turbomath::Vector wfinal = wbar - bias_ + kp * w_err; - - // - // Propagate Dynamics - // - - integrate_angular_rate(state_.attitude, wfinal, dt); - - // - // Post-Processing - // - - // Extract Euler Angles for controller - state_.attitude.get_RPY(&state_.roll, &state_.pitch, &state_.yaw); - - // Save off adjust gyro measurements with estimated biases for control - state_.angular_velocity = gyro_LPF_ - bias_; - - // If it has been more than 0.5 seconds since the accel update ran and we - // are supposed to be getting them then trigger an unhealthy estimator error. - if (RF_.params_.get_param_int(PARAM_FILTER_USE_ACC) && now_us > 500000 + last_acc_update_us_ - && !RF_.params_.get_param_int(PARAM_FIXED_WING)) { - RF_.state_manager_.set_error(StateManager::ERROR_UNHEALTHY_ESTIMATOR); - } else { - RF_.state_manager_.clear_error(StateManager::ERROR_UNHEALTHY_ESTIMATOR); - } -} - -bool Estimator::can_use_accel() const -{ - // if we are not using accel, just bail - if (!RF_.params_.get_param_int(PARAM_FILTER_USE_ACC)) return false; - - // current magnitude of LPF'd accelerometer - const float a_sqrd_norm = accel_LPF_.sqrd_norm(); - - // Ideally, gyros would never drift and we would never have to use the accelerometer. - // Since gyros do drift, we can use the accelerometer (in a non-accelerated state) as - // another estimate of roll/pitch angles and to make gyro biases observable (except r). - // Since there is noise, we give some margin to what a "non-accelerated state" means. - // Establish allowed acceleration deviation from 1g (i.e., non-accelerated flight). - const float margin = RF_.params_.get_param_float(PARAM_FILTER_ACCEL_MARGIN); - const float lowerbound = (1.0f - margin) * (1.0f - margin) * 9.80665f * 9.80665f; - const float upperbound = (1.0f + margin) * (1.0f + margin) * 9.80665f * 9.80665f; - - // if the magnitude of the accel measurement is close to 1g, we can use the - // accelerometer to correct roll and pitch and estimate gyro biases. - return (lowerbound < a_sqrd_norm && a_sqrd_norm < upperbound); -} - -bool Estimator::can_use_extatt() const { return extatt_update_next_run_; } - -turbomath::Vector Estimator::accel_correction() const -{ - // turn measurement into a unit vector - turbomath::Vector a = accel_LPF_.normalized(); - - // Get the quaternion from accelerometer (low-frequency measure q) - // (Not in either paper) - turbomath::Quaternion q_acc_inv(g_, a); - - // Get the error quaternion between observer and low-freq q - // Below Eq. 45 Mahony Paper - turbomath::Quaternion q_tilde = q_acc_inv * state_.attitude; - - // Correction Term of Eq. 47a and 47b Mahony Paper - // w_acc = 2*s_tilde*v_tilde - turbomath::Vector w_acc; - w_acc.x = -2.0f * q_tilde.w * q_tilde.x; - w_acc.y = -2.0f * q_tilde.w * q_tilde.y; - w_acc.z = 0.0f; // Don't correct z, because it's unobservable from the accelerometer - - return w_acc; -} - -turbomath::Vector Estimator::extatt_correction() const -{ - // DCM rows of attitude estimate and external measurement (world w.r.t body). - // These are the world axes from the perspective of the body frame. - // Note: If we extracted cols it would be body w.r.t world. - turbomath::Vector xhat_BW, yhat_BW, zhat_BW; - turbomath::Vector xext_BW, yext_BW, zext_BW; - - // extract rows of rotation matrix from quaternion attitude estimate - quaternion_to_dcm(state_.attitude, xhat_BW, yhat_BW, zhat_BW); - - // extract rows of rotation matrix from quaternion external attitude - quaternion_to_dcm(q_extatt_, xext_BW, yext_BW, zext_BW); - - // calculate cross products of corresponding axes as an error metric. For example, if vehicle were - // level but extatt had a different yaw angle than the internal estimate, xext_BW.cross(xhat_BW) - // would be a measure of how the filter needs to update in order to the internal estimate's yaw - // to closer to the extatt measurement. This is done for each axis. - turbomath::Vector w_ext = - xext_BW.cross(xhat_BW) + yext_BW.cross(yhat_BW) + zext_BW.cross(zhat_BW); - - return w_ext; -} - -turbomath::Vector Estimator::smoothed_gyro_measurement() -{ - turbomath::Vector wbar; - if (RF_.params_.get_param_int(PARAM_FILTER_USE_QUAD_INT)) { - // Quadratic Interpolation (Eq. 14 Casey Paper) - // this step adds 12 us on the STM32F10x chips - wbar = (w2_ / -12.0f) + w1_ * (8.0f / 12.0f) + gyro_LPF_ * (5.0f / 12.0f); - w2_ = w1_; - w1_ = gyro_LPF_; - } else { - wbar = gyro_LPF_; - } - - return wbar; -} - -void Estimator::integrate_angular_rate(turbomath::Quaternion & quat, - const turbomath::Vector & omega, const float dt) const -{ - // only propagate if we've moved - // TODO[PCL]: Will this ever be true? We should add a margin to this - const float sqrd_norm_w = omega.sqrd_norm(); - if (sqrd_norm_w == 0.0f) return; - - // for convenience - const float &p = omega.x, &q = omega.y, &r = omega.z; - - if (RF_.params_.get_param_int(PARAM_FILTER_USE_MAT_EXP)) { - // Matrix Exponential Approximation (From Attitude Representation and Kinematic - // Propagation for Low-Cost UAVs by Robert T. Casey) - // (Eq. 12 Casey Paper) - // This adds 90 us on STM32F10x chips - float norm_w = sqrtf(sqrd_norm_w); - float t1 = cosf((norm_w * dt) / 2.0f); - float t2 = 1.0f / norm_w * sinf((norm_w * dt) / 2.0f); - quat.w = t1 * quat.w + t2 * (-p * quat.x - q * quat.y - r * quat.z); - quat.x = t1 * quat.x + t2 * (p * quat.w + r * quat.y - q * quat.z); - quat.y = t1 * quat.y + t2 * (q * quat.w - r * quat.x + p * quat.z); - quat.z = t1 * quat.z + t2 * (r * quat.w + q * quat.x - p * quat.y); - quat.normalize(); - } else { - // Euler Integration - // (Eq. 47a Mahony Paper) - turbomath::Quaternion qdot( - 0.5f * (-p * quat.x - q * quat.y - r * quat.z), 0.5f * (p * quat.w + r * quat.y - q * quat.z), - 0.5f * (q * quat.w - r * quat.x + p * quat.z), 0.5f * (r * quat.w + q * quat.x - p * quat.y)); - quat.w += qdot.w * dt; - quat.x += qdot.x * dt; - quat.y += qdot.y * dt; - quat.z += qdot.z * dt; - quat.normalize(); - } -} - -void Estimator::quaternion_to_dcm(const turbomath::Quaternion & q, turbomath::Vector & X, - turbomath::Vector & Y, turbomath::Vector & Z) const -{ - // R(q) = [X.x X.y X.z] - // [Y.x Y.y Y.z] - // [Z.x Z.y Z.z] - - const float &w = q.w, &x = q.x, &y = q.y, &z = q.z; - X.x = 1.0f - 2.0f * (y * y + z * z); - X.y = 2.0f * (x * y - z * w); - X.z = 2.0f * (x * z + y * w); - Y.x = 2.0f * (x * y + z * w); - Y.y = 1.0f - 2.0f * (x * x + z * z); - Y.z = 2.0f * (y * z - x * w); - Z.x = 2.0f * (x * z - y * w); - Z.y = 2.0f * (y * z + x * w); - Z.z = 1.0f - 2.0f * (x * x + y * y); -} - -} // namespace rosflight_firmware diff --git a/src/main.cpp b/src/main.cpp deleted file mode 100644 index d3b69d73..00000000 --- a/src/main.cpp +++ /dev/null @@ -1,77 +0,0 @@ -/** - ****************************************************************************** - * File : main.cpp - * Date : Oct 5, 2023 - ****************************************************************************** - * - * Copyright (c) 2023, AeroVironment, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1.Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2.Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3.Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************** - **/ - -#include -#include - -// Select which board implementation to include based on cmake variable -#ifdef BUILD_VARMINT_BOARD -#include -extern Varmint varmint; -#endif -#ifndef BUILD_TEST_BOARD // Skip main function for gtest - -#ifdef __cplusplus -extern "C" { -#endif -int main(void); -#ifdef __cplusplus -} -#endif - -/** - * @fn int main(void) - * @brief Program Start - */ -int main(void) -{ - // Select which board implementation to instantiate based on cmake variable -#ifdef BUILD_VARMINT_BOARD - rosflight_firmware::Board * board = &varmint; -#endif - - // Rosflight base code - board->init_board(); - rosflight_firmware::Mavlink mavlink(*board); - rosflight_firmware::ROSflight firmware(*board, mavlink); - - firmware.init(); - - while (true) { firmware.run(); } -} - -#endif // BUILD_TEST_BOARD diff --git a/src/mixer.cpp b/src/mixer.cpp deleted file mode 100644 index 2df2c9cc..00000000 --- a/src/mixer.cpp +++ /dev/null @@ -1,207 +0,0 @@ -/* - * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ - -#include "mixer.h" - -#include "rosflight.h" - -#include - -namespace rosflight_firmware -{ -Mixer::Mixer(ROSflight & _rf) - : RF_(_rf) -{ - mixer_to_use_ = nullptr; -} - -void Mixer::init() { init_mixing(); } - -void Mixer::param_change_callback(uint16_t param_id) -{ - switch (param_id) { - case PARAM_MIXER: - init_mixing(); - break; - case PARAM_MOTOR_PWM_SEND_RATE: - case PARAM_RC_TYPE: - init_PWM(); - break; - default: - // do nothing - break; - } -} - -void Mixer::init_mixing() -{ - // clear the invalid mixer error - RF_.state_manager_.clear_error(StateManager::ERROR_INVALID_MIXER); - - uint8_t mixer_choice = RF_.params_.get_param_int(PARAM_MIXER); - - if (mixer_choice >= NUM_MIXERS) { - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Invalid Mixer Choice"); - - // set the invalid mixer flag - RF_.state_manager_.set_error(StateManager::ERROR_INVALID_MIXER); - mixer_to_use_ = nullptr; - } else { - mixer_to_use_ = array_of_mixers_[mixer_choice]; - } - - init_PWM(); - - for (int8_t i = 0; i < NUM_TOTAL_OUTPUTS; i++) { - raw_outputs_[i] = 0.0f; - outputs_[i] = 0.0f; - } -} - -void Mixer::init_PWM() -{ - uint32_t refresh_rate = RF_.params_.get_param_int(PARAM_MOTOR_PWM_SEND_RATE); - if (refresh_rate == 0 && mixer_to_use_ != nullptr) { - refresh_rate = mixer_to_use_->default_pwm_rate; - } - int16_t off_pwm = 1000; - - if (mixer_to_use_ == nullptr || refresh_rate == 0) RF_.board_.pwm_init(50, 0); - else - RF_.board_.pwm_init(refresh_rate, off_pwm); -} - -void Mixer::write_motor(uint8_t index, float value) -{ - if (RF_.state_manager_.state().armed) { - if (value > 1.0) { - value = 1.0; - } else if (value < RF_.params_.get_param_float(PARAM_MOTOR_IDLE_THROTTLE) - && RF_.params_.get_param_int(PARAM_SPIN_MOTORS_WHEN_ARMED)) { - value = RF_.params_.get_param_float(PARAM_MOTOR_IDLE_THROTTLE); - } else if (value < 0.0) { - value = 0.0; - } - } else { - value = 0.0; - } - raw_outputs_[index] = value; - RF_.board_.pwm_write(index, raw_outputs_[index]); -} - -void Mixer::write_servo(uint8_t index, float value) -{ - if (value > 1.0) { - value = 1.0; - } else if (value < -1.0) { - value = -1.0; - } - raw_outputs_[index] = value; - RF_.board_.pwm_write(index, raw_outputs_[index] * 0.5 + 0.5); -} - -void Mixer::set_new_aux_command(aux_command_t new_aux_command) -{ - for (uint8_t i = 0; i < NUM_TOTAL_OUTPUTS; i++) { - aux_command_.channel[i].type = new_aux_command.channel[i].type; - aux_command_.channel[i].value = new_aux_command.channel[i].value; - } -} - -void Mixer::mix_output() -{ - Controller::Output commands = RF_.controller_.output(); - float max_output = 1.0f; - - // Reverse fixed-wing channels just before mixing if we need to - if (RF_.params_.get_param_int(PARAM_FIXED_WING)) { - commands.x *= RF_.params_.get_param_int(PARAM_AILERON_REVERSE) ? -1 : 1; - commands.y *= RF_.params_.get_param_int(PARAM_ELEVATOR_REVERSE) ? -1 : 1; - commands.z *= RF_.params_.get_param_int(PARAM_RUDDER_REVERSE) ? -1 : 1; - } else if (commands.F < RF_.params_.get_param_float(PARAM_MOTOR_IDLE_THROTTLE)) { - // For multirotors, disregard yaw commands if throttle is low to prevent motor spin-up while - // arming/disarming - commands.z = 0.0; - } - - if (mixer_to_use_ == nullptr) return; - - for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) { - if (mixer_to_use_->output_type[i] != NONE) { - // Matrix multiply to mix outputs - outputs_[i] = (commands.F * mixer_to_use_->F[i] + commands.x * mixer_to_use_->x[i] - + commands.y * mixer_to_use_->y[i] + commands.z * mixer_to_use_->z[i]); - - // Save off the largest control output if it is greater than 1.0 for future scaling - if (outputs_[i] > max_output) { max_output = outputs_[i]; } - } - } - - // saturate outputs to maintain controllability even during aggressive maneuvers - float scale_factor = 1.0; - if (max_output > 1.0) { scale_factor = 1.0 / max_output; } - - // Perform Motor Output Scaling - for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) { - // scale all motor outputs by scale factor (this is usually 1.0, unless we saturated) - outputs_[i] *= scale_factor; - } - - // Insert AUX Commands, and assemble combined_output_types array (Does not override mixer values) - - // For the first NUM_MIXER_OUTPUTS channels, only write aux_command to channels the mixer is not - // using - for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) { - if (mixer_to_use_->output_type[i] == NONE) { - outputs_[i] = aux_command_.channel[i].value; - combined_output_type_[i] = aux_command_.channel[i].type; - } else { - combined_output_type_[i] = mixer_to_use_->output_type[i]; - } - } - - // The other channels are never used by the mixer - for (uint8_t i = NUM_MIXER_OUTPUTS; i < NUM_TOTAL_OUTPUTS; i++) { - outputs_[i] = aux_command_.channel[i].value; - combined_output_type_[i] = aux_command_.channel[i].type; - } - - // Write to outputs - for (uint8_t i = 0; i < NUM_TOTAL_OUTPUTS; i++) { - if (combined_output_type_[i] == S) { - write_servo(i, outputs_[i]); - } else if (combined_output_type_[i] == M) { - write_motor(i, outputs_[i]); - } - } -} - -} // namespace rosflight_firmware diff --git a/src/param.cpp b/src/param.cpp deleted file mode 100644 index 9d46608b..00000000 --- a/src/param.cpp +++ /dev/null @@ -1,360 +0,0 @@ -/* - * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ - -#include "param.h" - -#include "board.h" -#include "mixer.h" - -#include "rosflight.h" - -#include -#include - -namespace rosflight_firmware -{ -Params::Params(ROSflight & _rf) - : RF_(_rf) - , listeners_(nullptr) - , num_listeners_(0) -{} - -// local function definitions -void Params::init_param_int(uint16_t id, const char name[PARAMS_NAME_LENGTH], int32_t value) -{ - // copy cstr including '\0' or until maxlen - const uint8_t len = (strlen(name) >= PARAMS_NAME_LENGTH) ? PARAMS_NAME_LENGTH : strlen(name) + 1; - memcpy(params.names[id], name, len); - params.values[id].ivalue = value; - params.types[id] = PARAM_TYPE_INT32; -} - -void Params::init_param_float(uint16_t id, const char name[PARAMS_NAME_LENGTH], float value) -{ - // copy cstr including '\0' or until maxlen - const uint8_t len = (strlen(name) >= PARAMS_NAME_LENGTH) ? PARAMS_NAME_LENGTH : strlen(name) + 1; - memcpy(params.names[id], name, len); - params.values[id].fvalue = value; - params.types[id] = PARAM_TYPE_FLOAT; -} - -uint8_t Params::compute_checksum(void) -{ - uint8_t chk = 0; - const char * p; - - for (p = reinterpret_cast(¶ms.values); - p < reinterpret_cast(¶ms.values) + 4 * PARAMS_COUNT; p++) - chk ^= *p; - for (p = reinterpret_cast(¶ms.names); - p < reinterpret_cast(¶ms.names) + PARAMS_COUNT * PARAMS_NAME_LENGTH; p++) - chk ^= *p; - for (p = reinterpret_cast(¶ms.types); - p < reinterpret_cast(¶ms.types) + PARAMS_COUNT; p++) - chk ^= *p; - - return chk; -} - -// function definitions -void Params::init() -{ - RF_.board_.memory_init(); - if (!read()) { - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, - "Unable to load parameters; using default values"); - set_defaults(); - write(); - } -} - -// clang-format off -void Params::set_defaults(void) -{ - /******************************/ - /*** HARDWARE CONFIGURATION ***/ - /******************************/ - init_param_int(PARAM_BAUD_RATE, "BAUD_RATE", 921600); // Baud rate of MAVlink communication with companion computer | 9600 | 921600 - init_param_int(PARAM_SERIAL_DEVICE, "SERIAL_DEVICE", 0); // Serial Port (for supported devices) | 0 | 3 - - /*****************************/ - /*** MAVLINK CONFIGURATION ***/ - /*****************************/ - init_param_int(PARAM_SYSTEM_ID, "SYS_ID", 1); // Mavlink System ID | 1 | 255 - - /********************************/ - /*** CONTROLLER CONFIGURATION ***/ - /********************************/ - init_param_float(PARAM_MAX_COMMAND, "PARAM_MAX_CMD", 1.0); // saturation point for PID controller output | 0 | 1.0 - - init_param_float(PARAM_PID_ROLL_RATE_P, "PID_ROLL_RATE_P", 0.070f); // Roll Rate Proportional Gain | 0.0 | 1000.0 - init_param_float(PARAM_PID_ROLL_RATE_I, "PID_ROLL_RATE_I", 0.000f); // Roll Rate Integral Gain | 0.0 | 1000.0 - init_param_float(PARAM_PID_ROLL_RATE_D, "PID_ROLL_RATE_D", 0.000f); // Roll Rate Derivative Gain | 0.0 | 1000.0 - - init_param_float(PARAM_PID_PITCH_RATE_P, "PID_PITCH_RATE_P", 0.070f); // Pitch Rate Proportional Gain | 0.0 | 1000.0 - init_param_float(PARAM_PID_PITCH_RATE_I, "PID_PITCH_RATE_I", 0.0000f); // Pitch Rate Integral Gain | 0.0 | 1000.0 - init_param_float(PARAM_PID_PITCH_RATE_D, "PID_PITCH_RATE_D", 0.0000f); // Pitch Rate Derivative Gain | 0.0 | 1000.0 - - init_param_float(PARAM_PID_YAW_RATE_P, "PID_YAW_RATE_P", 0.25f); // Yaw Rate Proportional Gain | 0.0 | 1000.0 - init_param_float(PARAM_PID_YAW_RATE_I, "PID_YAW_RATE_I", 0.0f); // Yaw Rate Integral Gain | 0.0 | 1000.0 - init_param_float(PARAM_PID_YAW_RATE_D, "PID_YAW_RATE_D", 0.0f); // Yaw Rate Derivative Gain | 0.0 | 1000.0 - - init_param_float(PARAM_PID_ROLL_ANGLE_P, "PID_ROLL_ANG_P", 0.15f); // Roll Angle Proportional Gain | 0.0 | 1000.0 - init_param_float(PARAM_PID_ROLL_ANGLE_I, "PID_ROLL_ANG_I", 0.0f); // Roll Angle Integral Gain | 0.0 | 1000.0 - init_param_float(PARAM_PID_ROLL_ANGLE_D, "PID_ROLL_ANG_D", 0.05f); // Roll Angle Derivative Gain | 0.0 | 1000.0 - - init_param_float(PARAM_PID_PITCH_ANGLE_P, "PID_PITCH_ANG_P", 0.15f); // Pitch Angle Proportional Gain | 0.0 | 1000.0 - init_param_float(PARAM_PID_PITCH_ANGLE_I, "PID_PITCH_ANG_I", 0.0f); // Pitch Angle Integral Gain | 0.0 | 1000.0 - init_param_float(PARAM_PID_PITCH_ANGLE_D, "PID_PITCH_ANG_D", 0.05f); // Pitch Angle Derivative Gain | 0.0 | 1000.0 - - init_param_float(PARAM_X_EQ_TORQUE, "X_EQ_TORQUE", 0.0f); // Equilibrium torque added to output of controller on x axis | -1.0 | 1.0 - init_param_float(PARAM_Y_EQ_TORQUE, "Y_EQ_TORQUE", 0.0f); // Equilibrium torque added to output of controller on y axis | -1.0 | 1.0 - init_param_float(PARAM_Z_EQ_TORQUE, "Z_EQ_TORQUE", 0.0f); // Equilibrium torque added to output of controller on z axis | -1.0 | 1.0 - - init_param_float(PARAM_PID_TAU, "PID_TAU", 0.05f); // Dirty Derivative time constant - See controller documentation | 0.0 | 1.0 - - - /*************************/ - /*** PWM CONFIGURATION ***/ - /*************************/ - init_param_int(PARAM_MOTOR_PWM_SEND_RATE, "MOTOR_PWM_UPDATE", 0); // Overrides default PWM rate specified by mixer if non-zero - Requires reboot to take effect | 0 | 490 - init_param_float(PARAM_MOTOR_IDLE_THROTTLE, "MOTOR_IDLE_THR", 0.1); // min throttle command sent to motors when armed (Set above 0.1 to spin when armed) | 0.0 | 1.0 - init_param_float(PARAM_FAILSAFE_THROTTLE, "FAILSAFE_THR", -1.0); // Throttle sent to motors in failsafe condition (set just below hover throttle) | 0.0 | 1.0 - init_param_int(PARAM_SPIN_MOTORS_WHEN_ARMED, "ARM_SPIN_MOTORS", true); // Enforce MOTOR_IDLE_THR | 0 | 1 - - /*******************************/ - /*** ESTIMATOR CONFIGURATION ***/ - /*******************************/ - init_param_int(PARAM_INIT_TIME, "FILTER_INIT_T", 3000); // Time in ms to initialize estimator | 0 | 100000 - init_param_float(PARAM_FILTER_KP_ACC, "FILTER_KP_ACC", 0.5f); // estimator proportional gain on accel-based error - See estimator documentation | 0 | 10.0 - init_param_float(PARAM_FILTER_KI, "FILTER_KI", 0.01f); // estimator integral gain - See estimator documentation | 0 | 1.0 - init_param_float(PARAM_FILTER_KP_EXT, "FILTER_KP_EXT", 1.5f); // estimator proportional gain on external attitude-based error - See estimator documentation | 0 | 10.0 - init_param_float(PARAM_FILTER_ACCEL_MARGIN, "FILTER_ACCMARGIN", 0.1f); // allowable accel norm margin around 1g to determine if accel is usable | 0 | 1.0 - - init_param_int(PARAM_FILTER_USE_QUAD_INT, "FILTER_QUAD_INT", 1); // Perform a quadratic averaging of LPF gyro data prior to integration (adds ~20 us to estimation loop on F1 processors) | 0 | 1 - init_param_int(PARAM_FILTER_USE_MAT_EXP, "FILTER_MAT_EXP", 1); // 1 - Use matrix exponential to improve gyro integration (adds ~90 us to estimation loop in F1 processors) 0 - use euler integration | 0 | 1 - init_param_int(PARAM_FILTER_USE_ACC, "FILTER_USE_ACC", 1); // Use accelerometer to correct gyro integration drift (adds ~70 us to estimation loop) | 0 | 1 - - init_param_int(PARAM_CALIBRATE_GYRO_ON_ARM, "CAL_GYRO_ARM", false); // True if desired to calibrate gyros on arm | 0 | 1 - - init_param_float(PARAM_GYRO_XY_ALPHA, "GYROXY_LPF_ALPHA", 0.3f); // Low-pass filter constant on gyro X and Y axes - See estimator documentation | 0 | 1.0 - init_param_float(PARAM_GYRO_Z_ALPHA, "GYROZ_LPF_ALPHA", 0.3f); // Low-pass filter constant on gyro Z axis - See estimator documentation | 0 | 1.0 - init_param_float(PARAM_ACC_ALPHA, "ACC_LPF_ALPHA", 0.5f); // Low-pass filter constant on all accel axes - See estimator documentation | 0 | 1.0 - - init_param_float(PARAM_GYRO_X_BIAS, "GYRO_X_BIAS", 0.0f); // Constant x-bias of gyroscope readings | -1.0 | 1.0 - init_param_float(PARAM_GYRO_Y_BIAS, "GYRO_Y_BIAS", 0.0f); // Constant y-bias of gyroscope readings | -1.0 | 1.0 - init_param_float(PARAM_GYRO_Z_BIAS, "GYRO_Z_BIAS", 0.0f); // Constant z-bias of gyroscope readings | -1.0 | 1.0 - init_param_float(PARAM_ACC_X_BIAS, "ACC_X_BIAS", 0.0f); // Constant x-bias of accelerometer readings | -2.0 | 2.0 - init_param_float(PARAM_ACC_Y_BIAS, "ACC_Y_BIAS", 0.0f); // Constant y-bias of accelerometer readings | -2.0 | 2.0 - init_param_float(PARAM_ACC_Z_BIAS, "ACC_Z_BIAS", 0.0f); // Constant z-bias of accelerometer readings | -2.0 | 2.0 - init_param_float(PARAM_ACC_X_TEMP_COMP, "ACC_X_TEMP_COMP", 0.0f); // Linear x-axis temperature compensation constant | -2.0 | 2.0 - init_param_float(PARAM_ACC_Y_TEMP_COMP, "ACC_Y_TEMP_COMP", 0.0f); // Linear y-axis temperature compensation constant | -2.0 | 2.0 - init_param_float(PARAM_ACC_Z_TEMP_COMP, "ACC_Z_TEMP_COMP", 0.0f); // Linear z-axis temperature compensation constant | -2.0 | 2.0 - - init_param_float(PARAM_MAG_A11_COMP, "MAG_A11_COMP", 1.0f); // Soft iron compensation constant | -999.0 | 999.0 - init_param_float(PARAM_MAG_A12_COMP, "MAG_A12_COMP", 0.0f); // Soft iron compensation constant | -999.0 | 999.0 - init_param_float(PARAM_MAG_A13_COMP, "MAG_A13_COMP", 0.0f); // Soft iron compensation constant | -999.0 | 999.0 - init_param_float(PARAM_MAG_A21_COMP, "MAG_A21_COMP", 0.0f); // Soft iron compensation constant | -999.0 | 999.0 - init_param_float(PARAM_MAG_A22_COMP, "MAG_A22_COMP", 1.0f); // Soft iron compensation constant | -999.0 | 999.0 - init_param_float(PARAM_MAG_A23_COMP, "MAG_A23_COMP", 0.0f); // Soft iron compensation constant | -999.0 | 999.0 - init_param_float(PARAM_MAG_A31_COMP, "MAG_A31_COMP", 0.0f); // Soft iron compensation constant | -999.0 | 999.0 - init_param_float(PARAM_MAG_A32_COMP, "MAG_A32_COMP", 0.0f); // Soft iron compensation constant | -999.0 | 999.0 - init_param_float(PARAM_MAG_A33_COMP, "MAG_A33_COMP", 1.0f); // Soft iron compensation constant | -999.0 | 999.0 - init_param_float(PARAM_MAG_X_BIAS, "MAG_X_BIAS", 0.0f); // Hard iron compensation constant | -999.0 | 999.0 - init_param_float(PARAM_MAG_Y_BIAS, "MAG_Y_BIAS", 0.0f); // Hard iron compensation constant | -999.0 | 999.0 - init_param_float(PARAM_MAG_Z_BIAS, "MAG_Z_BIAS", 0.0f); // Hard iron compensation constant | -999.0 | 999.0 - - init_param_float(PARAM_BARO_BIAS, "BARO_BIAS", 0.0f); // Barometer measurement bias (Pa) | 0 | inf - init_param_float(PARAM_GROUND_LEVEL, "GROUND_LEVEL", 1387.0f); // Altitude of ground level (m) | -1000 | 10000 - - init_param_float(PARAM_DIFF_PRESS_BIAS, "DIFF_PRESS_BIAS", 0.0f); // Differential Pressure Bias (Pa) | -10 | 10 - - /************************/ - /*** RC CONFIGURATION ***/ - /************************/ - init_param_int(PARAM_RC_TYPE, "RC_TYPE", 0); // Type of RC input 0 - PPM, 1 - SBUS | 0 | 1 - init_param_int(PARAM_RC_X_CHANNEL, "RC_X_CHN", 0); // RC input channel mapped to x-axis commands [0 - indexed] | 0 | 3 - init_param_int(PARAM_RC_Y_CHANNEL, "RC_Y_CHN", 1); // RC input channel mapped to y-axis commands [0 - indexed] | 0 | 3 - init_param_int(PARAM_RC_Z_CHANNEL, "RC_Z_CHN", 3); // RC input channel mapped to z-axis commands [0 - indexed] | 0 | 3 - init_param_int(PARAM_RC_F_CHANNEL, "RC_F_CHN", 2); // RC input channel mapped to F-axis commands [0 - indexed] | 0 | 3 - init_param_int(PARAM_RC_ATTITUDE_OVERRIDE_CHANNEL, "RC_ATT_OVRD_CHN", 4); // RC switch mapped to attitude override [0 indexed, -1 to disable] | 4 | 7 - init_param_int(PARAM_RC_THROTTLE_OVERRIDE_CHANNEL, "RC_THR_OVRD_CHN", 4); // RC switch channel mapped to throttle override [0 indexed, -1 to disable] | 4 | 7 - init_param_int(PARAM_RC_ATT_CONTROL_TYPE_CHANNEL, "RC_ATT_CTRL_CHN",-1); // RC switch channel mapped to attitude control type [0 indexed, -1 to disable] | 4 | 7 - init_param_int(PARAM_RC_ARM_CHANNEL, "ARM_CHANNEL",-1); // RC switch channel mapped to arming (only if PARAM_ARM_STICKS is false) [0 indexed, -1 to disable] | 4 | 7 - init_param_int(PARAM_RC_NUM_CHANNELS, "RC_NUM_CHN", 6); // number of RC input channels | 1 | 8 - - init_param_int(PARAM_RC_SWITCH_5_DIRECTION, "SWITCH_5_DIR", 1); // RC switch 5 toggle direction | -1 | 1 - init_param_int(PARAM_RC_SWITCH_6_DIRECTION, "SWITCH_6_DIR", 1); // RC switch 6 toggle direction | -1 | 1 - init_param_int(PARAM_RC_SWITCH_7_DIRECTION, "SWITCH_7_DIR", 1); // RC switch 7 toggle direction | -1 | 1 - init_param_int(PARAM_RC_SWITCH_8_DIRECTION, "SWITCH_8_DIR", 1); // RC switch 8 toggle direction | -1 | 1 - - init_param_float(PARAM_RC_OVERRIDE_DEVIATION, "RC_OVRD_DEV", 0.1); // RC stick deviation from center for override | 0.0 | 1.0 - init_param_int(PARAM_OVERRIDE_LAG_TIME, "OVRD_LAG_TIME", 1000); // RC stick deviation lag time before returning control (ms) | 0 | 100000 - init_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, "MIN_THROTTLE", true); // Take minimum throttle between RC and computer at all times | 0 | 1 - - init_param_int(PARAM_RC_ATTITUDE_MODE, "RC_ATT_MODE", 1); // Attitude mode for RC sticks (0: rate, 1: angle). Overridden if RC_ATT_CTRL_CHN is set. | 0 | 1 - init_param_float(PARAM_RC_MAX_ROLL, "RC_MAX_ROLL", 0.786f); // Maximum roll angle command sent by full deflection of RC sticks | 0.0 | 3.14159 - init_param_float(PARAM_RC_MAX_PITCH, "RC_MAX_PITCH", 0.786f); // Maximum pitch angle command sent by full stick deflection of RC sticks | 0.0 | 3.14159 - init_param_float(PARAM_RC_MAX_ROLLRATE, "RC_MAX_ROLLRATE", 3.14159f); // Maximum roll rate command sent by full stick deflection of RC sticks | 0.0 | 9.42477796077 - init_param_float(PARAM_RC_MAX_PITCHRATE, "RC_MAX_PITCHRATE", 3.14159f); // Maximum pitch command sent by full stick deflection of RC sticks | 0.0 | 3.14159 - init_param_float(PARAM_RC_MAX_YAWRATE, "RC_MAX_YAWRATE", 1.507f); // Maximum pitch command sent by full stick deflection of RC sticks | 0.0 | 3.14159 - - /***************************/ - /*** FRAME CONFIGURATION ***/ - /***************************/ - init_param_int(PARAM_MIXER, "MIXER", Mixer::INVALID_MIXER); // Which mixer to choose - See Mixer documentation | 0 | 10 - - init_param_int(PARAM_FIXED_WING, "FIXED_WING", false); // switches on pass-through commands for fixed-wing operation | 0 | 1 - init_param_int(PARAM_ELEVATOR_REVERSE, "ELEVATOR_REV", 0); // reverses elevator servo output | 0 | 1 - init_param_int(PARAM_AILERON_REVERSE, "AIL_REV", 0); // reverses aileron servo output | 0 | 1 - init_param_int(PARAM_RUDDER_REVERSE, "RUDDER_REV", 0); // reverses rudder servo output | 0 | 1 - - init_param_float(PARAM_FC_ROLL, "FC_ROLL", 0.0f); // roll angle (deg) of flight controller wrt aircraft body | 0 | 360 - init_param_float(PARAM_FC_PITCH, "FC_PITCH", 0.0f); // pitch angle (deg) of flight controller wrt aircraft body | 0 | 360 - init_param_float(PARAM_FC_YAW, "FC_YAW", 0.0f); // yaw angle (deg) of flight controller wrt aircraft body | 0 | 360 - - - /********************/ - /*** ARMING SETUP ***/ - /********************/ - init_param_float(PARAM_ARM_THRESHOLD, "ARM_THRESHOLD", 0.15); // RC deviation from max/min in yaw and throttle for arming and disarming check (us) | 0 | 500 - - /*****************************/ - /*** BATTERY MONITOR SETUP ***/ - /*****************************/ - init_param_float(PARAM_BATTERY_VOLTAGE_MULTIPLIER, "BATT_VOLT_MULT", 0.0f); - init_param_float(PARAM_BATTERY_CURRENT_MULTIPLIER, "BATT_CURR_MULT", 0.0f); - init_param_float(PARAM_BATTERY_VOLTAGE_ALPHA, "BATT_VOLT_ALPHA", 0.995f); - init_param_float(PARAM_BATTERY_CURRENT_ALPHA, "BATT_CURR_ALPHA", 0.995f); - - /************************/ - /*** OFFBOARD CONTROL ***/ - /************************/ - init_param_int(PARAM_OFFBOARD_TIMEOUT, "OFFBOARD_TIMEOUT", 100); // Timeout in milliseconds for offboard commands, after which RC override is activated | 0 | 100000 -} -// clang-format on - -void Params::set_listeners(ParamListenerInterface * const listeners[], size_t num_listeners) -{ - listeners_ = listeners; - num_listeners_ = num_listeners; -} - -bool Params::read(void) -{ - if (!RF_.board_.memory_read(¶ms, sizeof(params_t))) return false; - - if (params.version != GIT_VERSION_HASH) return false; - - if (params.size != sizeof(params_t) || params.magic_be != 0xBE || params.magic_ef != 0xEF) - return false; - - if (compute_checksum() != params.chk) return false; - - return true; -} - -bool Params::write(void) -{ - params.version = GIT_VERSION_HASH; - params.size = sizeof(params_t); - params.magic_be = 0xBE; - params.magic_ef = 0xEF; - params.chk = compute_checksum(); - - if (!RF_.board_.memory_write(¶ms, sizeof(params_t))) return false; - return true; -} - -void Params::change_callback(uint16_t id) -{ - // call the callback function for all listeners - if (listeners_ != nullptr) { - for (size_t i = 0; i < num_listeners_; i++) { listeners_[i]->param_change_callback(id); } - } -} - -uint16_t Params::lookup_param_id(const char name[PARAMS_NAME_LENGTH]) -{ - for (uint16_t id = 0; id < PARAMS_COUNT; id++) { - bool match = true; - for (uint8_t i = 0; i < PARAMS_NAME_LENGTH; i++) { - // compare each character - if (name[i] != params.names[id][i]) { - match = false; - break; - } - - // stop comparing if end of string is reached - if (params.names[id][i] == '\0') break; - } - - if (match) return id; - } - - return PARAMS_COUNT; -} - -bool Params::set_param_int(uint16_t id, int32_t value) -{ - if (id < PARAMS_COUNT && value != params.values[id].ivalue) { - params.values[id].ivalue = value; - change_callback(id); - RF_.comm_manager_.send_param_value(id); - return true; - } - return false; -} - -bool Params::set_param_float(uint16_t id, float value) -{ - if (id < PARAMS_COUNT && value != params.values[id].fvalue) { - params.values[id].fvalue = value; - change_callback(id); - RF_.comm_manager_.send_parameter_list(); - return true; - } - return false; -} - -bool Params::set_param_by_name_int(const char name[PARAMS_NAME_LENGTH], int32_t value) -{ - uint16_t id = lookup_param_id(name); - return set_param_int(id, value); -} - -bool Params::set_param_by_name_float(const char name[PARAMS_NAME_LENGTH], float value) -{ - param_value_t tmp; - tmp.fvalue = value; - return set_param_by_name_int(name, tmp.ivalue); -} -} // namespace rosflight_firmware diff --git a/src/rc.cpp b/src/rc.cpp deleted file mode 100644 index d90364b8..00000000 --- a/src/rc.cpp +++ /dev/null @@ -1,282 +0,0 @@ -/* - * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ - -#include "rc.h" - -#include "rosflight.h" - -#include - -namespace rosflight_firmware -{ -RC::RC(ROSflight & _rf) - : RF_(_rf) -{} - -void RC::init() -{ - init_rc(); - new_command_ = false; -} - -void RC::init_rc() -{ - RF_.board_.rc_init(static_cast(RF_.params_.get_param_int(PARAM_RC_TYPE))); - init_sticks(); - init_switches(); -} - -void RC::param_change_callback(uint16_t param_id) -{ - switch (param_id) { - case PARAM_RC_TYPE: - RF_.board_.rc_init(static_cast(RF_.params_.get_param_int(PARAM_RC_TYPE))); - break; - case PARAM_RC_X_CHANNEL: - case PARAM_RC_Y_CHANNEL: - case PARAM_RC_Z_CHANNEL: - case PARAM_RC_F_CHANNEL: - init_sticks(); - break; - case PARAM_RC_ATTITUDE_OVERRIDE_CHANNEL: - case PARAM_RC_THROTTLE_OVERRIDE_CHANNEL: - case PARAM_RC_ATT_CONTROL_TYPE_CHANNEL: - case PARAM_RC_ARM_CHANNEL: - case PARAM_RC_SWITCH_5_DIRECTION: - case PARAM_RC_SWITCH_6_DIRECTION: - case PARAM_RC_SWITCH_7_DIRECTION: - case PARAM_RC_SWITCH_8_DIRECTION: - init_switches(); - break; - default: - // do nothing - break; - } -} - -float RC::stick(Stick channel) { return stick_values[channel]; } - -bool RC::switch_on(Switch channel) { return switch_values[channel]; } - -bool RC::switch_mapped(Switch channel) { return switches[channel].mapped; } - -void RC::init_sticks(void) -{ - sticks[STICK_X].channel = RF_.params_.get_param_int(PARAM_RC_X_CHANNEL); - sticks[STICK_X].one_sided = false; - - sticks[STICK_Y].channel = RF_.params_.get_param_int(PARAM_RC_Y_CHANNEL); - sticks[STICK_Y].one_sided = false; - - sticks[STICK_Z].channel = RF_.params_.get_param_int(PARAM_RC_Z_CHANNEL); - sticks[STICK_Z].one_sided = false; - - sticks[STICK_F].channel = RF_.params_.get_param_int(PARAM_RC_F_CHANNEL); - sticks[STICK_F].one_sided = true; -} - -void RC::init_switches() -{ - for (uint8_t chan = 0; chan < static_cast(SWITCHES_COUNT); chan++) { - char channel_name[18]; - switch (chan) { - case SWITCH_ARM: - strcpy(channel_name, "ARM"); - switches[chan].channel = RF_.params_.get_param_int(PARAM_RC_ARM_CHANNEL); - break; - case SWITCH_ATT_OVERRIDE: - strcpy(channel_name, "ATTITUDE OVERRIDE"); - switches[chan].channel = RF_.params_.get_param_int(PARAM_RC_ATTITUDE_OVERRIDE_CHANNEL); - break; - case SWITCH_THROTTLE_OVERRIDE: - strcpy(channel_name, "THROTTLE OVERRIDE"); - switches[chan].channel = RF_.params_.get_param_int(PARAM_RC_THROTTLE_OVERRIDE_CHANNEL); - break; - case SWITCH_ATT_TYPE: - strcpy(channel_name, "ATTITUDE TYPE"); - switches[chan].channel = RF_.params_.get_param_int(PARAM_RC_ATT_CONTROL_TYPE_CHANNEL); - break; - default: - strcpy(channel_name, "INVALID"); - switches[chan].channel = 255; - break; - } - - switches[chan].mapped = switches[chan].channel > 3 - && switches[chan].channel < RF_.params_.get_param_int(PARAM_RC_NUM_CHANNELS); - - switch (switches[chan].channel) { - case 4: - switches[chan].direction = RF_.params_.get_param_int(PARAM_RC_SWITCH_5_DIRECTION); - break; - case 5: - switches[chan].direction = RF_.params_.get_param_int(PARAM_RC_SWITCH_6_DIRECTION); - break; - case 6: - switches[chan].direction = RF_.params_.get_param_int(PARAM_RC_SWITCH_7_DIRECTION); - break; - case 7: - switches[chan].direction = RF_.params_.get_param_int(PARAM_RC_SWITCH_8_DIRECTION); - break; - default: - switches[chan].direction = 1; - break; - } - - if (switches[chan].mapped) - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO, - "%s switch mapped to RC channel %d", channel_name, - switches[chan].channel); - else - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO, "%s switch not mapped", - channel_name); - } -} - -bool RC::check_rc_lost() -{ - bool failsafe = false; - - // If the board reports that we have lost RC, tell the state manager - if (RF_.board_.rc_lost()) { - failsafe = true; - } else { - // go into failsafe if we get an invalid RC command for any channel - for (int8_t i = 0; i < RF_.params_.get_param_int(PARAM_RC_NUM_CHANNELS); i++) { - float pwm = RF_.board_.rc_read(i); - if (pwm < -0.25 || pwm > 1.25) { failsafe = true; } - } - } - - if (failsafe) - // set the RC Lost error flag - RF_.state_manager_.set_event(StateManager::EVENT_RC_LOST); - else - // Clear the RC Lost Error - RF_.state_manager_.set_event(StateManager::EVENT_RC_FOUND); - - return failsafe; -} - -void RC::look_for_arm_disarm_signal() -{ - uint32_t now_ms = RF_.board_.clock_millis(); - uint32_t dt = now_ms - prev_time_ms; - prev_time_ms = now_ms; - // check for arming switch - if (!switch_mapped(SWITCH_ARM)) { - if (!RF_.state_manager_.state().armed) // we are DISARMED - { - // if left stick is down and to the right - if ((RF_.rc_.stick(STICK_F) < RF_.params_.get_param_float(PARAM_ARM_THRESHOLD)) - && (RF_.rc_.stick(STICK_Z) > (1.0f - RF_.params_.get_param_float(PARAM_ARM_THRESHOLD)))) { - time_sticks_have_been_in_arming_position_ms += dt; - } else { - time_sticks_have_been_in_arming_position_ms = 0; - } - if (time_sticks_have_been_in_arming_position_ms > 1000) { - RF_.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); - } - } else // we are ARMED - { - // if left stick is down and to the left - if (RF_.rc_.stick(STICK_F) < RF_.params_.get_param_float(PARAM_ARM_THRESHOLD) - && RF_.rc_.stick(STICK_Z) < -(1.0f - RF_.params_.get_param_float(PARAM_ARM_THRESHOLD))) { - time_sticks_have_been_in_arming_position_ms += dt; - } else { - time_sticks_have_been_in_arming_position_ms = 0; - } - if (time_sticks_have_been_in_arming_position_ms > 1000) { - RF_.state_manager_.set_event(StateManager::EVENT_REQUEST_DISARM); - time_sticks_have_been_in_arming_position_ms = 0; - } - } - } else // ARMING WITH SWITCH - { - if (RF_.rc_.switch_on(SWITCH_ARM)) { - if (!RF_.state_manager_.state().armed) - RF_.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); - ; - } else { - RF_.state_manager_.set_event(StateManager::EVENT_REQUEST_DISARM); - } - } -} - -bool RC::run() -{ - // Check for rc lost - if (check_rc_lost()) return false; - - // read and normalize stick values - for (uint8_t channel = 0; channel < static_cast(STICKS_COUNT); channel++) { - float pwm = RF_.board_.rc_read(sticks[channel].channel); - if (sticks[channel].one_sided) // generally only F is one_sided - { - stick_values[channel] = pwm; - } else { - stick_values[channel] = 2.0 * (pwm - 0.5); - } - } - - // read and interpret switch values - for (uint8_t channel = 0; channel < static_cast(SWITCHES_COUNT); channel++) { - if (switches[channel].mapped) { - float pwm = RF_.board_.rc_read(switches[channel].channel); - if (switches[channel].direction < 0) { - switch_values[channel] = pwm < 0.2; - } else { - switch_values[channel] = pwm >= 0.8; - } - } else { - switch_values[channel] = false; - } - } - - // Look for arming and disarming signals - look_for_arm_disarm_signal(); - - // Signal to the mux that we need to compute a new combined command - new_command_ = true; - return true; -} - -bool RC::new_command() -{ - if (new_command_) { - new_command_ = false; - return true; - } else - return false; - ; -} - -} // namespace rosflight_firmware diff --git a/src/rosflight.cpp b/src/rosflight.cpp deleted file mode 100644 index 017b1bd9..00000000 --- a/src/rosflight.cpp +++ /dev/null @@ -1,141 +0,0 @@ -/* - * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ - -#include "rosflight.h" - -#include "interface/param_listener.h" - -namespace rosflight_firmware -{ -ROSflight::ROSflight(Board & board, CommLinkInterface & comm_link) - : board_(board) - , comm_manager_(*this, comm_link) - , params_(*this) - , command_manager_(*this) - , controller_(*this) - , estimator_(*this) - , mixer_(*this) - , rc_(*this) - , sensors_(*this) - , state_manager_(*this) -{ - comm_link.set_listener(&comm_manager_); - params_.set_listeners(param_listeners_, num_param_listeners_); -} - -// Initialization Routine -void ROSflight::init() -{ - // Initialize the arming finite state machine - state_manager_.init(); - - // Read EEPROM to get initial params - params_.init(); - - // Initialize Mixer - mixer_.init(); - - /***********************/ - /*** Hardware Setup ***/ - /***********************/ - - // Initialize PWM and RC - rc_.init(); - - // Initialize MAVlink Communication - comm_manager_.init(); - - // Initialize Sensors - sensors_.init(); - - /***********************/ - /*** Software Setup ***/ - /***********************/ - - // Initialize Estimator - estimator_.init(); - - // Initialize Controller - controller_.init(); - - // Initialize the command muxer - command_manager_.init(); - - /***************************/ - /*** Hardfault Recovery ***/ - /***************************/ - - state_manager_.check_backup_memory(); -} - -/** - * @fn void run() - * @brief Main Loop - * - */ -void ROSflight::run() -{ - /*********************/ - /*** Control Loop ***/ - /*********************/ - uint64_t start = board_.clock_micros(); - - got_flags got = sensors_.run(); // IMU, GNSS, Baro, Mag, Pitot, SONAR, Battery - - if (got.imu) { - estimator_.run(); - controller_.run(); - mixer_.mix_output(); - loop_time_us = board_.clock_micros() - start; - } - - /*********************/ - /*** Post-Process ***/ - /*********************/ - // internal timers figure out what and when to send - comm_manager_.stream(got); - - // receive mavlink messages - comm_manager_.receive(); - - // update the state machine, an internal timer runs this at a fixed rate - state_manager_.run(); - - // get RC, synchronous with rc data acquisition - if (got.rc) rc_.run(); - - // update commands (internal logic tells whether or not we should do anything or not) - command_manager_.run(); -} - -uint32_t ROSflight::get_loop_time_us() { return loop_time_us; } - -} // namespace rosflight_firmware diff --git a/src/sensors.cpp b/src/sensors.cpp deleted file mode 100644 index e23a48b1..00000000 --- a/src/sensors.cpp +++ /dev/null @@ -1,626 +0,0 @@ -/* - * Copyright (c) 2017, James Jackson, Daniel Koch, and Craig Bidstrup, - * BYU MAGICC Lab - * - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ - -#include "sensors.h" - -#include "param.h" - -#include "rosflight.h" - -#include - -#include -#include -#include - -namespace rosflight_firmware -{ -// TODO: These values don't change actual rates, is there a way to just reference actual rates -// as defined in hardware board implementation? -const float Sensors::BARO_MAX_CHANGE_RATE = 200.0f; // approx 200 m/s -const float Sensors::BARO_SAMPLE_RATE = 60.0f; -const float Sensors::DIFF_MAX_CHANGE_RATE = 225.0f; // approx 15 m/s^2 -const float Sensors::DIFF_SAMPLE_RATE = 100.0f; -const float Sensors::SONAR_MAX_CHANGE_RATE = 100.0f; // 100 m/s -const float Sensors::SONAR_SAMPLE_RATE = 50.0f; - -const int Sensors::SENSOR_CAL_DELAY_CYCLES = 128; -const int Sensors::SENSOR_CAL_CYCLES = 127; - -const float Sensors::BARO_MAX_CALIBRATION_VARIANCE = 25.0; // standard dev about 0.2 m -const float Sensors::DIFF_PRESSURE_MAX_CALIBRATION_VARIANCE = 100.0; // standard dev about 3 m/s - -Sensors::Sensors(ROSflight & rosflight) - : rf_(rosflight) -{} - -void Sensors::init() -{ - new_imu_data_ = false; - - // clear the IMU read error - rf_.state_manager_.clear_error(StateManager::ERROR_IMU_NOT_RESPONDING); - rf_.board_.sensors_init(); - - init_imu(); - - next_sensor_to_update_ = BAROMETER; - - float alt = rf_.params_.get_param_float(PARAM_GROUND_LEVEL); - ground_pressure_ = 101325.0f * static_cast(pow((1 - 2.25694e-5 * alt), 5.2553)); - - baro_outlier_filt_.init(BARO_MAX_CHANGE_RATE, BARO_SAMPLE_RATE, ground_pressure_); - diff_outlier_filt_.init(DIFF_MAX_CHANGE_RATE, DIFF_SAMPLE_RATE, 0.0f); - sonar_outlier_filt_.init(SONAR_MAX_CHANGE_RATE, SONAR_SAMPLE_RATE, 0.0f); - int_start_us_ = rf_.board_.clock_micros(); - - this->update_battery_monitor_multipliers(); -} - -void Sensors::init_imu() -{ - // Quaternion to compensate for FCU orientation - float roll = rf_.params_.get_param_float(PARAM_FC_ROLL) * 0.017453293; - float pitch = rf_.params_.get_param_float(PARAM_FC_PITCH) * 0.017453293; - float yaw = rf_.params_.get_param_float(PARAM_FC_YAW) * 0.017453293; - data_.fcu_orientation = turbomath::Quaternion(roll, pitch, yaw); - - // See if the IMU is uncalibrated, and throw an error if it is - if (rf_.params_.get_param_float(PARAM_ACC_X_BIAS) == 0.0 - && rf_.params_.get_param_float(PARAM_ACC_Y_BIAS) == 0.0 - && rf_.params_.get_param_float(PARAM_ACC_Z_BIAS) == 0.0 - && rf_.params_.get_param_float(PARAM_GYRO_X_BIAS) == 0.0 - && rf_.params_.get_param_float(PARAM_GYRO_Y_BIAS) == 0.0 - && rf_.params_.get_param_float(PARAM_GYRO_Z_BIAS) == 0.0) { - rf_.state_manager_.set_error(StateManager::ERROR_UNCALIBRATED_IMU); - } -} - -void Sensors::param_change_callback(uint16_t param_id) -{ - switch (param_id) { - case PARAM_FC_ROLL: - case PARAM_FC_PITCH: - case PARAM_FC_YAW: - init_imu(); - break; - case PARAM_BATTERY_VOLTAGE_MULTIPLIER: - case PARAM_BATTERY_CURRENT_MULTIPLIER: - update_battery_monitor_multipliers(); - break; - case PARAM_BATTERY_VOLTAGE_ALPHA: - battery_voltage_alpha_ = rf_.params_.get_param_float(PARAM_BATTERY_VOLTAGE_ALPHA); - break; - case PARAM_BATTERY_CURRENT_ALPHA: - battery_current_alpha_ = rf_.params_.get_param_float(PARAM_BATTERY_CURRENT_ALPHA); - break; - default: - // do nothing - break; - } -} - -got_flags Sensors::run() -{ - memset(&got, 0, sizeof(got)); - - // IMU: - if (rf_.board_.imu_has_new_data()) { - got.imu = true; - rf_.state_manager_.clear_error(StateManager::ERROR_IMU_NOT_RESPONDING); - last_imu_update_ms_ = rf_.board_.clock_millis(); - - if (!rf_.board_.imu_read(accel_, &data_.imu_temperature, gyro_, &data_.imu_time)) - got.imu = false; - - // Move data into local copy - data_.accel.x = accel_[0]; - data_.accel.y = accel_[1]; - data_.accel.z = accel_[2]; - - data_.accel = data_.fcu_orientation * data_.accel; - - data_.gyro.x = gyro_[0]; - data_.gyro.y = gyro_[1]; - data_.gyro.z = gyro_[2]; - - data_.gyro = data_.fcu_orientation * data_.gyro; - - if (calibrating_acc_flag_) calibrate_accel(); - if (calibrating_gyro_flag_) calibrate_gyro(); - - // Apply bias correction - correct_imu(); - - // Integrate for filtered IMU - float dt = (data_.imu_time - prev_imu_read_time_us_) * 1e-6; - accel_int_ += dt * data_.accel; - gyro_int_ += dt * data_.gyro; - prev_imu_read_time_us_ = data_.imu_time; - } - - // GNSS: - if (rf_.board_.gnss_present()) { - data_.gnss_present = true; - if (rf_.board_.gnss_has_new_data()) { - got.gnss = true; - rf_.board_.gnss_read(&this->data_.gnss_data, &this->data_.gnss_full); - } - got.gnss_full = got.gnss; // bot come with the pvt GPS data - } - - // BAROMETER: - if (rf_.board_.baro_present()) { - data_.baro_present = true; - if (rf_.board_.baro_has_new_data()) { - got.baro = true; - float raw_pressure; - float raw_temp; - rf_.board_.baro_read(&raw_pressure, &raw_temp); - data_.baro_valid = baro_outlier_filt_.update(raw_pressure, &data_.baro_pressure); - if (data_.baro_valid) { - data_.baro_temperature = raw_temp; - correct_baro(); - } - } - } - - // MAGNETOMETER: - if (rf_.board_.mag_present()) { - data_.mag_present = true; - float mag[3]; - if (rf_.board_.mag_has_new_data()) { - got.mag = true; - rf_.board_.mag_read(mag); - data_.mag.x = mag[0]; - data_.mag.y = mag[1]; - data_.mag.z = mag[2]; - correct_mag(); - } - } - - // DIFF_PRESSURE: - if (rf_.board_.diff_pressure_present()) { - data_.diff_pressure_present = true; - if (rf_.board_.diff_pressure_has_new_data()) { - got.diff_pressure = true; - float raw_pressure; - float raw_temp; - rf_.board_.diff_pressure_read(&raw_pressure, &raw_temp); - data_.diff_pressure_valid = diff_outlier_filt_.update(raw_pressure, &data_.diff_pressure); - if (data_.diff_pressure_valid) { - data_.diff_pressure_temp = raw_temp; - correct_diff_pressure(); - } - } - } - - // SONAR: - if (rf_.board_.sonar_present()) { - data_.sonar_present = true; - if (rf_.board_.sonar_has_new_data()) { - got.sonar = true; - float raw_distance; - rf_.board_.sonar_read(&raw_distance); - data_.sonar_range_valid = sonar_outlier_filt_.update(raw_distance, &data_.sonar_range); - } - } - - // BATTERY_MONITOR: - if (rf_.board_.battery_has_new_data()) { - got.battery = true; - last_battery_monitor_update_ms_ = rf_.board_.clock_millis(); - update_battery_monitor(); - } - - // RC - if (rf_.board_.rc_has_new_data()) { - got.rc = true; - rf_.board_.rc_read(0); - } - - return got; -} - -void Sensors::update_other_sensors() {} - -void Sensors::look_for_disabled_sensors() {} - -bool Sensors::start_imu_calibration(void) -{ - start_gyro_calibration(); - - calibrating_acc_flag_ = true; - rf_.params_.set_param_float(PARAM_ACC_X_BIAS, 0.0); - rf_.params_.set_param_float(PARAM_ACC_Y_BIAS, 0.0); - rf_.params_.set_param_float(PARAM_ACC_Z_BIAS, 0.0); - return true; -} - -bool Sensors::start_gyro_calibration(void) -{ - calibrating_gyro_flag_ = true; - rf_.params_.set_param_float(PARAM_GYRO_X_BIAS, 0.0); - rf_.params_.set_param_float(PARAM_GYRO_Y_BIAS, 0.0); - rf_.params_.set_param_float(PARAM_GYRO_Z_BIAS, 0.0); - return true; -} - -bool Sensors::start_baro_calibration() -{ - baro_calibration_mean_ = 0.0f; - baro_calibration_var_ = 0.0f; - baro_calibration_count_ = 0; - baro_calibrated_ = false; - rf_.params_.set_param_float(PARAM_BARO_BIAS, 0.0f); - return true; -} - -bool Sensors::start_diff_pressure_calibration() -{ - diff_pressure_calibration_mean_ = 0.0f; - diff_pressure_calibration_var_ = 0.0f; - diff_pressure_calibration_count_ = 0; - diff_pressure_calibrated_ = false; - rf_.params_.set_param_float(PARAM_DIFF_PRESS_BIAS, 0.0f); - return true; -} - -bool Sensors::gyro_calibration_complete(void) { return !calibrating_gyro_flag_; } - -//================================================================== -// local function definitions -bool Sensors::update_imu(void) -{ - bool new_data; - if ((new_data = rf_.board_.imu_has_new_data()) > 0) { - rf_.state_manager_.clear_error(StateManager::ERROR_IMU_NOT_RESPONDING); - last_imu_update_ms_ = rf_.board_.clock_millis(); - if (!rf_.board_.imu_read(accel_, &data_.imu_temperature, gyro_, &data_.imu_time)) return false; - - // Move data into local copy - data_.accel.x = accel_[0]; - data_.accel.y = accel_[1]; - data_.accel.z = accel_[2]; - - data_.accel = data_.fcu_orientation * data_.accel; - - data_.gyro.x = gyro_[0]; - data_.gyro.y = gyro_[1]; - data_.gyro.z = gyro_[2]; - - data_.gyro = data_.fcu_orientation * data_.gyro; - - if (calibrating_acc_flag_) calibrate_accel(); - if (calibrating_gyro_flag_) calibrate_gyro(); - - // Apply bias correction - correct_imu(); - - // Integrate for filtered IMU - float dt = (data_.imu_time - prev_imu_read_time_us_) * 1e-6; - accel_int_ += dt * data_.accel; - gyro_int_ += dt * data_.gyro; - prev_imu_read_time_us_ = data_.imu_time; - } - return new_data; -} - -void Sensors::get_filtered_IMU(turbomath::Vector & accel, turbomath::Vector & gyro, - uint64_t & stamp_us) -{ - float delta_t = (data_.imu_time - int_start_us_) * 1e-6; - accel = accel_int_ / delta_t; - gyro = gyro_int_ / delta_t; - accel_int_ *= 0.0; - gyro_int_ *= 0.0; - int_start_us_ = data_.imu_time; - stamp_us = data_.imu_time; -} - -void Sensors::update_battery_monitor() -{ - if (rf_.board_.battery_present()) { - float battery_voltage, battery_current; - rf_.board_.battery_read(&battery_voltage, &battery_current); - data_.battery_monitor_present = true; - data_.battery_voltage = battery_voltage; - data_.battery_current = battery_current; - } -} - -//====================================================================== -// Calibration Functions -void Sensors::calibrate_gyro() -{ - gyro_sum_ += data_.gyro; - gyro_calibration_count_++; - - if (gyro_calibration_count_ > 1000) { - // Gyros are simple. Just find the average during the calibration - turbomath::Vector gyro_bias = gyro_sum_ / static_cast(gyro_calibration_count_); - - if (gyro_bias.norm() < 1.0) { - rf_.params_.set_param_float(PARAM_GYRO_X_BIAS, gyro_bias.x); - rf_.params_.set_param_float(PARAM_GYRO_Y_BIAS, gyro_bias.y); - rf_.params_.set_param_float(PARAM_GYRO_Z_BIAS, gyro_bias.z); - - // Tell the estimator to reset it's bias estimate, because it should be zero now - rf_.estimator_.reset_adaptive_bias(); - - // Tell the state manager that we just completed a gyro calibration - rf_.state_manager_.set_event(StateManager::EVENT_CALIBRATION_COMPLETE); - } else { - // Tell the state manager that we just failed a gyro calibration - rf_.state_manager_.set_event(StateManager::EVENT_CALIBRATION_FAILED); - rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, - "Too much movement for gyro cal"); - } - - // reset calibration in case we do it again - calibrating_gyro_flag_ = false; - gyro_calibration_count_ = 0; - gyro_sum_.x = 0.0f; - gyro_sum_.y = 0.0f; - gyro_sum_.z = 0.0f; - } -} - -turbomath::Vector vector_max(turbomath::Vector a, turbomath::Vector b) -{ - return turbomath::Vector(a.x > b.x ? a.x : b.x, a.y > b.y ? a.y : b.y, a.z > b.z ? a.z : b.z); -} - -turbomath::Vector vector_min(turbomath::Vector a, turbomath::Vector b) -{ - return turbomath::Vector(a.x < b.x ? a.x : b.x, a.y < b.y ? a.y : b.y, a.z < b.z ? a.z : b.z); -} - -void Sensors::calibrate_accel(void) -{ - acc_sum_ = acc_sum_ + data_.accel + gravity_; - acc_temp_sum_ += data_.imu_temperature; - max_ = vector_max(max_, data_.accel); - min_ = vector_min(min_, data_.accel); - accel_calibration_count_++; - - if (accel_calibration_count_ > 1000) { - // The temperature bias is calculated using a least-squares regression. - // This is computationally intensive, so it is done by the companion - // computer in fcu_io and shipped over to the flight controller. - turbomath::Vector accel_temp_bias = {rf_.params_.get_param_float(PARAM_ACC_X_TEMP_COMP), - rf_.params_.get_param_float(PARAM_ACC_Y_TEMP_COMP), - rf_.params_.get_param_float(PARAM_ACC_Z_TEMP_COMP)}; - - // Figure out the proper accel bias. - // We have to consider the contribution of temperature during the calibration, - // Which is why this line is so confusing. What we are doing, is first removing - // the contribution of temperature to the measurements during the calibration, - // Then we are dividing by the number of measurements. - turbomath::Vector accel_bias = - (acc_sum_ - (accel_temp_bias * acc_temp_sum_)) / static_cast(accel_calibration_count_); - - // Sanity Check - - // If the accelerometer is upside down or being spun around during the calibration, - // then don't do anything - if ((max_ - min_).norm() > 1.0) { - rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, - "Too much movement for IMU cal"); - calibrating_acc_flag_ = false; - } else { - // reset the estimated state - rf_.estimator_.reset_state(); - calibrating_acc_flag_ = false; - - if (accel_bias.norm() < 3.0) { - rf_.params_.set_param_float(PARAM_ACC_X_BIAS, accel_bias.x); - rf_.params_.set_param_float(PARAM_ACC_Y_BIAS, accel_bias.y); - rf_.params_.set_param_float(PARAM_ACC_Z_BIAS, accel_bias.z); - rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO, "IMU offsets captured"); - - // clear uncalibrated IMU flag - rf_.state_manager_.clear_error(StateManager::ERROR_UNCALIBRATED_IMU); - } else { - // This usually means the user has the FCU in the wrong orientation, or something is wrong - // with the board IMU (like it's a cheap chinese clone) - rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, - "large accel bias: norm = %d.%d", - static_cast(accel_bias.norm()), - static_cast(accel_bias.norm() * 1000) % 1000); - } - } - - // reset calibration counters in case we do it again - accel_calibration_count_ = 0; - acc_sum_.x = 0.0f; - acc_sum_.y = 0.0f; - acc_sum_.z = 0.0f; - acc_temp_sum_ = 0.0f; - max_.x = -1000.0f; - max_.y = -1000.0f; - max_.z = -1000.0f; - min_.x = 1000.0f; - min_.y = 1000.0f; - min_.z = 1000.0f; - } -} - -void Sensors::calibrate_baro() -{ - if (rf_.board_.clock_millis() > last_baro_cal_iter_ms_ + 20) { - baro_calibration_count_++; - - // calibrate pressure reading to where it should be - if (baro_calibration_count_ > SENSOR_CAL_DELAY_CYCLES + SENSOR_CAL_CYCLES) { - // if sample variance within acceptable range, flag calibration as done - // else reset cal variables and start over - if (baro_calibration_var_ < BARO_MAX_CALIBRATION_VARIANCE) { - rf_.params_.set_param_float(PARAM_BARO_BIAS, baro_calibration_mean_); - baro_calibrated_ = true; - rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO, "Baro Cal successful!"); - } else { - rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, - "Too much movement for barometer cal"); - } - baro_calibration_mean_ = 0.0f; - baro_calibration_var_ = 0.0f; - baro_calibration_count_ = 0; - } - - else if (baro_calibration_count_ > SENSOR_CAL_DELAY_CYCLES) { - float measurement = data_.baro_pressure - ground_pressure_; - float delta = measurement - baro_calibration_mean_; - baro_calibration_mean_ += delta / (baro_calibration_count_ - SENSOR_CAL_DELAY_CYCLES); - float delta2 = measurement - baro_calibration_mean_; - baro_calibration_var_ += delta * delta2 / (SENSOR_CAL_CYCLES - 1); - } - last_baro_cal_iter_ms_ = rf_.board_.clock_millis(); - } -} - -void Sensors::calibrate_diff_pressure() -{ - if (rf_.board_.clock_millis() > last_diff_pressure_cal_iter_ms_ + 20) { - diff_pressure_calibration_count_++; - - if (diff_pressure_calibration_count_ > SENSOR_CAL_DELAY_CYCLES + SENSOR_CAL_CYCLES) { - // if sample variance within acceptable range, flag calibration as done - // else reset cal variables and start over - if (diff_pressure_calibration_var_ < DIFF_PRESSURE_MAX_CALIBRATION_VARIANCE) { - rf_.params_.set_param_float(PARAM_DIFF_PRESS_BIAS, diff_pressure_calibration_mean_); - diff_pressure_calibrated_ = true; - rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO, "Airspeed Cal Successful!"); - } else { - rf_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, - "Too much movement for diff pressure cal"); - } - diff_pressure_calibration_mean_ = 0.0f; - diff_pressure_calibration_var_ = 0.0f; - diff_pressure_calibration_count_ = 0; - } else if (diff_pressure_calibration_count_ > SENSOR_CAL_DELAY_CYCLES) { - float delta = data_.diff_pressure - diff_pressure_calibration_mean_; - diff_pressure_calibration_mean_ += - delta / (diff_pressure_calibration_count_ - SENSOR_CAL_DELAY_CYCLES); - float delta2 = data_.diff_pressure - diff_pressure_calibration_mean_; - diff_pressure_calibration_var_ += delta * delta2 / (SENSOR_CAL_CYCLES - 1); - } - last_diff_pressure_cal_iter_ms_ = rf_.board_.clock_millis(); - } -} - -//====================================================== -// Correction Functions (These apply calibration constants) -void Sensors::correct_imu(void) -{ - // correct according to known biases and temperature compensation - data_.accel.x -= rf_.params_.get_param_float(PARAM_ACC_X_TEMP_COMP) * data_.imu_temperature - + rf_.params_.get_param_float(PARAM_ACC_X_BIAS); - data_.accel.y -= rf_.params_.get_param_float(PARAM_ACC_Y_TEMP_COMP) * data_.imu_temperature - + rf_.params_.get_param_float(PARAM_ACC_Y_BIAS); - data_.accel.z -= rf_.params_.get_param_float(PARAM_ACC_Z_TEMP_COMP) * data_.imu_temperature - + rf_.params_.get_param_float(PARAM_ACC_Z_BIAS); - - data_.gyro.x -= rf_.params_.get_param_float(PARAM_GYRO_X_BIAS); - data_.gyro.y -= rf_.params_.get_param_float(PARAM_GYRO_Y_BIAS); - data_.gyro.z -= rf_.params_.get_param_float(PARAM_GYRO_Z_BIAS); -} - -void Sensors::correct_mag(void) -{ - // correct according to known hard iron bias - float mag_hard_x = data_.mag.x - rf_.params_.get_param_float(PARAM_MAG_X_BIAS); - float mag_hard_y = data_.mag.y - rf_.params_.get_param_float(PARAM_MAG_Y_BIAS); - float mag_hard_z = data_.mag.z - rf_.params_.get_param_float(PARAM_MAG_Z_BIAS); - - // correct according to known soft iron bias - converts to nT - data_.mag.x = rf_.params_.get_param_float(PARAM_MAG_A11_COMP) * mag_hard_x - + rf_.params_.get_param_float(PARAM_MAG_A12_COMP) * mag_hard_y - + rf_.params_.get_param_float(PARAM_MAG_A13_COMP) * mag_hard_z; - data_.mag.y = rf_.params_.get_param_float(PARAM_MAG_A21_COMP) * mag_hard_x - + rf_.params_.get_param_float(PARAM_MAG_A22_COMP) * mag_hard_y - + rf_.params_.get_param_float(PARAM_MAG_A23_COMP) * mag_hard_z; - data_.mag.z = rf_.params_.get_param_float(PARAM_MAG_A31_COMP) * mag_hard_x - + rf_.params_.get_param_float(PARAM_MAG_A32_COMP) * mag_hard_y - + rf_.params_.get_param_float(PARAM_MAG_A33_COMP) * mag_hard_z; -} - -void Sensors::correct_baro(void) -{ - if (!baro_calibrated_) calibrate_baro(); - data_.baro_pressure -= rf_.params_.get_param_float(PARAM_BARO_BIAS); - data_.baro_altitude = - turbomath::alt(data_.baro_pressure) - rf_.params_.get_param_float(PARAM_GROUND_LEVEL); -} - -void Sensors::correct_diff_pressure() -{ - if (!diff_pressure_calibrated_) calibrate_diff_pressure(); - data_.diff_pressure -= rf_.params_.get_param_float(PARAM_DIFF_PRESS_BIAS); - - float atm = 101325.0f; - if (data_.baro_present) atm = data_.baro_pressure; - data_.diff_pressure_velocity = turbomath::fsign(data_.diff_pressure) * 24.574f - / turbomath::inv_sqrt((turbomath::fabs(data_.diff_pressure) * data_.diff_pressure_temp / atm)); -} - -void Sensors::OutlierFilter::init(float max_change_rate, float update_rate, float center) -{ - max_change_ = max_change_rate / update_rate; - window_size_ = 1; - center_ = center; - init_ = true; -} - -bool Sensors::OutlierFilter::update(float new_val, float * val) -{ - float diff = new_val - center_; - if (fabsf(diff) < window_size_ * max_change_) { - *val = new_val; - - center_ += turbomath::fsign(diff) * fminf(max_change_, fabsf(diff)); - if (window_size_ > 1) { window_size_--; } - return true; - } else { - window_size_++; - return false; - } -} - -void Sensors::update_battery_monitor_multipliers() -{ - float voltage_multiplier = this->rf_.params_.get_param_float(PARAM_BATTERY_VOLTAGE_MULTIPLIER); - float current_multiplier = this->rf_.params_.get_param_float(PARAM_BATTERY_CURRENT_MULTIPLIER); - this->rf_.board_.battery_voltage_set_multiplier(voltage_multiplier); - this->rf_.board_.battery_current_set_multiplier(current_multiplier); -} - -} // namespace rosflight_firmware diff --git a/src/state_manager.cpp b/src/state_manager.cpp deleted file mode 100644 index 3cfdbbaa..00000000 --- a/src/state_manager.cpp +++ /dev/null @@ -1,343 +0,0 @@ -/* - * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ - -#include "state_manager.h" - -#include "rosflight.h" - -namespace rosflight_firmware -{ -StateManager::StateManager(ROSflight & parent) - : RF_(parent) - , fsm_state_(FSM_STATE_INIT) -{ - state_.armed = false; - state_.error = false; - state_.failsafe = false; - state_.error_codes = 0x00; -} - -void StateManager::init() -{ - RF_.board_.backup_memory_init(); - - set_event(EVENT_INITIALIZED); - process_errors(); - - // Initialize LEDs - RF_.board_.led1_off(); -} - -void StateManager::run() -{ - // I'm putting this here for the case where we've switched states with existing errors, - // but those errors might not have been processed yet for the new state. We could replace - // this with a recursive call to process_errors() if the state changed in update_fsm()? - process_errors(); // check for any error events - update_leds(); -} - -void StateManager::set_error(uint16_t error) -{ - // Set the error code - state_.error_codes |= error; - - // Tell the FSM that we have had an error change - process_errors(); -} - -void StateManager::clear_error(uint16_t error) -{ - // If this error code was set, - if (state_.error_codes & error) { - // Clear the error code - state_.error_codes &= ~(error); - - // If there are no errors, tell the FSM - process_errors(); - - // Send a status update (for logging) - RF_.comm_manager_.update_status(); - } -} - -void StateManager::set_event(StateManager::Event event) -{ - FsmState start_state = fsm_state_; - uint16_t start_errors = state_.error_codes; - switch (fsm_state_) { - case FSM_STATE_INIT: - if (event == EVENT_INITIALIZED) { fsm_state_ = FSM_STATE_PREFLIGHT; } - break; - - case FSM_STATE_PREFLIGHT: - switch (event) { - case EVENT_RC_FOUND: - clear_error(ERROR_RC_LOST); - state_.failsafe = false; - break; - case EVENT_RC_LOST: - set_error(ERROR_RC_LOST); - break; - case EVENT_ERROR: - state_.error = true; - fsm_state_ = FSM_STATE_ERROR; - break; - case EVENT_REQUEST_ARM: - // require low RC throttle to arm - if (RF_.rc_.stick(RC::Stick::STICK_F) - < RF_.params_.get_param_float(PARAM_ARM_THRESHOLD)) { - // require either min throttle to be enabled or throttle override switch to be on - if (RF_.params_.get_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE) - || RF_.rc_.switch_on(RC::Switch::SWITCH_THROTTLE_OVERRIDE)) { - if (RF_.params_.get_param_int(PARAM_CALIBRATE_GYRO_ON_ARM)) { - fsm_state_ = FSM_STATE_CALIBRATING; - RF_.sensors_.start_gyro_calibration(); - } else { - state_.armed = true; - RF_.comm_manager_.update_status(); - fsm_state_ = FSM_STATE_ARMED; - } - } else { - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, - "RC throttle override must be active to arm"); - } - } else { - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, - "Cannot arm with RC throttle high"); - } - break; - default: - break; - } - break; - - case FSM_STATE_ERROR: - switch (event) { - case EVENT_RC_LOST: - set_error( - ERROR_RC_LOST); // sometimes redundant, but reports RC lost error if another error - // got reported first - break; - case EVENT_RC_FOUND: - clear_error(ERROR_RC_LOST); - state_.failsafe = false; - break; - case EVENT_NO_ERROR: - state_.error = false; - fsm_state_ = FSM_STATE_PREFLIGHT; - break; - case EVENT_REQUEST_ARM: - if (next_arming_error_msg_ms_ < RF_.board_.clock_millis()) { - if (state_.error_codes & StateManager::ERROR_INVALID_MIXER) - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, - "Unable to arm: Invalid mixer"); - if (state_.error_codes & StateManager::ERROR_IMU_NOT_RESPONDING) - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, - "Unable to arm: IMU not responding"); - if (state_.error_codes & StateManager::ERROR_RC_LOST) - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, - "Unable to arm: RC signal lost"); - if (state_.error_codes & StateManager::ERROR_UNHEALTHY_ESTIMATOR) - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, - "Unable to arm: Unhealthy estimator"); - if (state_.error_codes & StateManager::ERROR_TIME_GOING_BACKWARDS) - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, - "Unable to arm: Time going backwards"); - if (state_.error_codes & StateManager::ERROR_UNCALIBRATED_IMU) - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, - "Unable to arm: IMU not calibrated"); - if (state_.error_codes & StateManager::ERROR_INVALID_FAILSAFE) - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, - "Unable to arm: Invalid failsafe setting"); - - next_arming_error_msg_ms_ = - RF_.board_.clock_millis() + 1000; // throttle messages to 1 Hz - } - break; - default: - break; - } - break; - - case FSM_STATE_CALIBRATING: - switch (event) { - case EVENT_CALIBRATION_COMPLETE: - state_.armed = true; - fsm_state_ = FSM_STATE_ARMED; - break; - case EVENT_CALIBRATION_FAILED: - fsm_state_ = FSM_STATE_PREFLIGHT; - break; - case EVENT_RC_LOST: - set_error(ERROR_RC_LOST); - break; - case EVENT_ERROR: - fsm_state_ = FSM_STATE_ERROR; - state_.error = true; - break; - case EVENT_NO_ERROR: - state_.error = false; - break; - default: - break; - } - break; - - case FSM_STATE_ARMED: - switch (event) { - case EVENT_RC_LOST: - state_.failsafe = true; - fsm_state_ = FSM_STATE_FAILSAFE; - set_error(ERROR_RC_LOST); - RF_.comm_manager_.update_status(); - break; - case EVENT_REQUEST_DISARM: - state_.armed = false; - if (state_.error) fsm_state_ = FSM_STATE_ERROR; - else - fsm_state_ = FSM_STATE_PREFLIGHT; - break; - case EVENT_ERROR: - state_.error = true; - break; - case EVENT_NO_ERROR: - state_.error = false; - break; - default: - break; - } - break; - - case FSM_STATE_FAILSAFE: - switch (event) { - case EVENT_ERROR: - state_.error = true; - break; - case EVENT_REQUEST_DISARM: - state_.armed = false; - fsm_state_ = FSM_STATE_ERROR; - break; - case EVENT_RC_FOUND: - state_.failsafe = false; - fsm_state_ = FSM_STATE_ARMED; - clear_error(ERROR_RC_LOST); - break; - default: - break; - } - break; - default: - break; - } - - // If there has been a change, then report it to the user - if (start_state != fsm_state_ || state_.error_codes != start_errors) - RF_.comm_manager_.update_status(); -} - -void StateManager::write_backup_data(const BackupData::DebugInfo & debug) -{ - BackupData data; - data.reset_count = hardfault_count_ + 1; - data.error_code = state_.error_codes; - data.arm_flag = state_.armed ? BackupData::ARM_MAGIC : 0; - data.debug = debug; - - data.finalize(); - RF_.board_.backup_memory_write(reinterpret_cast(&data), sizeof(data)); -} - -void StateManager::check_backup_memory() -{ - // reinitialize to make sure backup memory is in a good state - RF_.board_.backup_memory_init(); - - // check for hardfault recovery data in backup memory - BackupData data; - if (RF_.board_.backup_memory_read(reinterpret_cast(&data), sizeof(data))) { - if (data.valid_checksum()) { - hardfault_count_ = data.reset_count; - - if (data.arm_flag == BackupData::ARM_MAGIC) { - // do emergency rearm if in a good state - if (fsm_state_ == FSM_STATE_PREFLIGHT) { - state_.armed = true; - fsm_state_ = FSM_STATE_ARMED; - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_CRITICAL, - "Rearming after hardfault!!!"); - } else { - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_CRITICAL, - "Failed to rearm after hardfault!!!"); - } - } - - // queue sending backup data over comm link - RF_.comm_manager_.send_backup_data(data); - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_CRITICAL, - "Recovered from hardfault!!!"); - } - - RF_.board_.backup_memory_clear(sizeof(data)); - } -} - -void StateManager::process_errors() -{ - if (state_.error_codes) set_event(EVENT_ERROR); - else - set_event(EVENT_NO_ERROR); -} - -void StateManager::update_leds() -{ - // blink fast if in failsafe - if (state_.failsafe) { - if (next_led_blink_ms_ < RF_.board_.clock_millis()) { - RF_.board_.led1_toggle(); - next_led_blink_ms_ = RF_.board_.clock_millis() + 100; - } - } - // blink slowly if in error - else if (state_.error) { - if (next_led_blink_ms_ < RF_.board_.clock_millis()) { - RF_.board_.led1_toggle(); - next_led_blink_ms_ = RF_.board_.clock_millis() + 500; - } - } - // off if disarmed, on if armed - else if (!state_.armed) - RF_.board_.led1_off(); - else - RF_.board_.led1_on(); -} - -} // namespace rosflight_firmware