diff --git a/.github/workflows/pre-release.yml b/.github/workflows/pre-release.yml index 04676287..cae3bfef 100644 --- a/.github/workflows/pre-release.yml +++ b/.github/workflows/pre-release.yml @@ -16,7 +16,7 @@ jobs: - name: checkout submodules run: git submodule update --init --recursive - name: install toolchain - run: sudo apt -y install gcc-arm-none-eabi + run: sudo apt -y install gcc-arm-none-eabi libeigen3-dev - name: check toolchain run: arm-none-eabi-gcc --version - name: build varmint diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml index 26bdfb0c..65cb84f8 100644 --- a/.github/workflows/release.yml +++ b/.github/workflows/release.yml @@ -16,7 +16,7 @@ jobs: - name: checkout submodules run: git submodule update --init --recursive - name: install toolchain - run: sudo apt -y install gcc-arm-none-eabi + run: sudo apt -y install gcc-arm-none-eabi libeigen3-dev - name: check toolchain run: arm-none-eabi-gcc --version - name: build varmint diff --git a/CMakeLists.txt b/CMakeLists.txt index 3efea7c0..799163d0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,8 +28,16 @@ if("${GIT_VERSION_HASH}" STREQUAL "") set(GIT_VERSION_HASH "0") endif() -### source files ### +### Install Eigen dependency ### +include(FetchContent) +FetchContent_Declare( + Eigen3 + GIT_REPOSITORY https://gitlab.com/libeigen/eigen.git + GIT_TAG origin/3.4 +) +FetchContent_MakeAvaliable(Eigen3) +### source files ### include_directories( include include/interface @@ -37,7 +45,7 @@ include_directories( comms/mavlink comms/mavlink/v1.0 comms/mavlink/v1.0/common - comms/mavlink/v1.0/rosflight + ${eigen3_SOURCE_DIR} ) file(GLOB_RECURSE ROSFLIGHT_SOURCES diff --git a/comms/mavlink/mavlink.cpp b/comms/mavlink/mavlink.cpp index 9217c89b..707a3961 100644 --- a/comms/mavlink/mavlink.cpp +++ b/comms/mavlink/mavlink.cpp @@ -487,15 +487,19 @@ void Mavlink::handle_msg_offboard_control(const mavlink_message_t * const msg) return; } - control.x.value = ctrl.x; - control.y.value = ctrl.y; - control.z.value = ctrl.z; - control.F.value = ctrl.F; - - control.x.valid = !(ctrl.ignore & IGNORE_VALUE1); - control.y.valid = !(ctrl.ignore & IGNORE_VALUE2); - control.z.valid = !(ctrl.ignore & IGNORE_VALUE3); - control.F.valid = !(ctrl.ignore & IGNORE_VALUE4); + control.Qx.value = ctrl.Qx; + control.Qy.value = ctrl.Qy; + control.Qz.value = ctrl.Qz; + control.Fx.value = ctrl.Fx; + control.Fy.value = ctrl.Fy; + control.Fz.value = ctrl.Fz; + + control.Qx.valid = !(ctrl.ignore & IGNORE_VALUE1); + control.Qy.valid = !(ctrl.ignore & IGNORE_VALUE2); + control.Qz.valid = !(ctrl.ignore & IGNORE_VALUE3); + control.Fx.valid = !(ctrl.ignore & IGNORE_VALUE4); + control.Fy.valid = !(ctrl.ignore & IGNORE_VALUE5); + control.Fz.valid = !(ctrl.ignore & IGNORE_VALUE6); if (listener_ != nullptr) { listener_->offboard_control_callback(control); } } diff --git a/include/command_manager.h b/include/command_manager.h index 4f39564e..c3f668b8 100644 --- a/include/command_manager.h +++ b/include/command_manager.h @@ -47,7 +47,7 @@ typedef enum { RATE, // Channel is is in rate mode (mrad/s) ANGLE, // Channel command is in angle mode (mrad) - THROTTLE, // Channel is direcly controlling throttle max/1000 + THROTTLE, // Channel is controlling throttle setting, which will be converted to force PASSTHROUGH, // Channel directly passes PWM input to the mixer } control_type_t; @@ -58,13 +58,22 @@ typedef struct float value; // The value of the channel } control_channel_t; +typedef enum +{ + X_AXIS, + Y_AXIS, + Z_AXIS, +} rc_f_axis_t; + typedef struct { uint32_t stamp_ms; - control_channel_t x; - control_channel_t y; - control_channel_t z; - control_channel_t F; + control_channel_t Qx; + control_channel_t Qy; + control_channel_t Qz; + control_channel_t Fx; + control_channel_t Fy; + control_channel_t Fz; } control_t; class CommandManager : public ParamListenerInterface @@ -77,38 +86,50 @@ class CommandManager : public ParamListenerInterface control_channel_t * combined; } mux_t; - mux_t muxes[4] = {{&rc_command_.x, &offboard_command_.x, &combined_command_.x}, - {&rc_command_.y, &offboard_command_.y, &combined_command_.y}, - {&rc_command_.z, &offboard_command_.z, &combined_command_.z}, - {&rc_command_.F, &offboard_command_.F, &combined_command_.F}}; + mux_t muxes[6] = {{&rc_command_.Qx, &offboard_command_.Qx, &combined_command_.Qx}, + {&rc_command_.Qy, &offboard_command_.Qy, &combined_command_.Qy}, + {&rc_command_.Qz, &offboard_command_.Qz, &combined_command_.Qz}, + {&rc_command_.Fx, &offboard_command_.Fx, &combined_command_.Fx}, + {&rc_command_.Fy, &offboard_command_.Fy, &combined_command_.Fy}, + {&rc_command_.Fz, &offboard_command_.Fz, &combined_command_.Fz}}; // clang-format off control_t rc_command_ = {0, {false, ANGLE, 0.0}, {false, ANGLE, 0.0}, {false, RATE, 0.0}, + {false, THROTTLE, 0.0}, + {false, THROTTLE, 0.0}, {false, THROTTLE, 0.0}}; control_t offboard_command_ = {0, {false, ANGLE, 0.0}, {false, ANGLE, 0.0}, {false, RATE, 0.0}, + {false, THROTTLE, 0.0}, + {false, THROTTLE, 0.0}, {false, THROTTLE, 0.0}}; control_t combined_command_ = {0, {false, ANGLE, 0.0}, {false, ANGLE, 0.0}, {false, RATE, 0.0}, + {false, THROTTLE, 0.0}, + {false, THROTTLE, 0.0}, {false, THROTTLE, 0.0}}; control_t multirotor_failsafe_command_ = {0, {true, ANGLE, 0.0}, {true, ANGLE, 0.0}, {true, RATE, 0.0}, + {true, THROTTLE, 0.0}, + {true, THROTTLE, 0.0}, {true, THROTTLE, 0.0}}; control_t fixedwing_failsafe_command_ = {0, {true, PASSTHROUGH, 0.0}, {true, PASSTHROUGH, 0.0}, {true, PASSTHROUGH, 0.0}, - {true, THROTTLE, 0.0}}; + {true, PASSTHROUGH, 0.0}, + {true, PASSTHROUGH, 0.0}, + {true, PASSTHROUGH, 0.0}}; // clang-format on typedef enum @@ -119,10 +140,12 @@ class CommandManager : public ParamListenerInterface enum MuxChannel { - MUX_X, - MUX_Y, - MUX_Z, - MUX_F, + MUX_QX, + MUX_QY, + MUX_QZ, + MUX_FX, + MUX_FY, + MUX_FZ, }; typedef struct @@ -138,7 +161,8 @@ class CommandManager : public ParamListenerInterface ROSflight & RF_; bool new_command_; - bool rc_override_; + bool rc_throttle_override_; + bool rc_attitude_override_; control_t & failsafe_command_; @@ -157,6 +181,8 @@ class CommandManager : public ParamListenerInterface void init(); bool run(); bool rc_override_active(); + bool rc_throttle_override_active(); + bool rc_attitude_override_active(); bool offboard_control_active(); void set_new_offboard_command(control_t new_offboard_command); void set_new_rc_command(control_t new_rc_command); diff --git a/include/controller.h b/include/controller.h index b8a9e9d1..c997c3e9 100644 --- a/include/controller.h +++ b/include/controller.h @@ -51,21 +51,26 @@ class Controller : public ParamListenerInterface public: struct Output { - float F; - float x; - float y; - float z; + float Fx; + float Fy; + float Fz; + float Qx; + float Qy; + float Qz; }; Controller(ROSflight & rf); inline const Output & output() const { return output_; } + inline float max_thrust() const { return max_thrust_; } void init(); void run(); + void calculate_max_thrust(); void calculate_equilbrium_torque_from_rc(); void param_change_callback(uint16_t param_id) override; + bool is_throttle_high(float threshold); private: class PID @@ -92,8 +97,8 @@ class Controller : public ParamListenerInterface ROSflight & RF_; - turbomath::Vector run_pid_loops(uint32_t dt, const Estimator::State & state, - const control_t & command, bool update_integrators); + Controller::Output run_pid_loops(uint32_t dt, const Estimator::State & state, + const control_t & command, bool update_integrators); Output output_; @@ -103,6 +108,8 @@ class Controller : public ParamListenerInterface PID pitch_rate_; PID yaw_rate_; + float max_thrust_; + uint64_t prev_time_us_; }; diff --git a/include/interface/comm_link.h b/include/interface/comm_link.h index c5a156e0..dc76162d 100644 --- a/include/interface/comm_link.h +++ b/include/interface/comm_link.h @@ -85,10 +85,12 @@ class CommLinkInterface }; Mode mode; - Channel x; - Channel y; - Channel z; - Channel F; + Channel Qx; + Channel Qy; + Channel Qz; + Channel Fx; + Channel Fy; + Channel Fz; }; struct AuxCommand diff --git a/include/mixer.h b/include/mixer.h index 8e2efc0c..3c3e1a98 100644 --- a/include/mixer.h +++ b/include/mixer.h @@ -33,6 +33,10 @@ #define ROSFLIGHT_FIRMWARE_MIXER_H #include "interface/param_listener.h" +#include "controller.h" + +#include +#include #include #include @@ -47,7 +51,7 @@ class Mixer : public ParamListenerInterface static constexpr uint8_t NUM_TOTAL_OUTPUTS = 14; static constexpr uint8_t NUM_MIXER_OUTPUTS = 10; - enum + typedef enum { ESC_CALIBRATION = 0, QUADCOPTER_PLUS = 1, @@ -58,19 +62,16 @@ class Mixer : public ParamListenerInterface OCTO_X = 6, Y6 = 7, X8 = 8, - TRICOPTER = 9, - FIXEDWING = 10, - PASSTHROUGH = 11, - VTAIL = 12, - QUADPLANE = 13, - CUSTOM = 14, + FIXEDWING = 9, + INVERTED_VTAIL = 10, + CUSTOM = 11, NUM_MIXERS, INVALID_MIXER = 255 - }; + } mixer_type_t; typedef enum { - NONE, // None + AUX, // None S, // Servo M, // Motor G // GPIO @@ -80,12 +81,26 @@ class Mixer : public ParamListenerInterface { output_type_t output_type[NUM_MIXER_OUTPUTS]; 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]; + float Fx[NUM_MIXER_OUTPUTS]; + float Fy[NUM_MIXER_OUTPUTS]; + float Fz[NUM_MIXER_OUTPUTS]; + float Qx[NUM_MIXER_OUTPUTS]; + float Qy[NUM_MIXER_OUTPUTS]; + float Qz[NUM_MIXER_OUTPUTS]; } mixer_t; + typedef struct + { + output_type_t (*output_type)[NUM_MIXER_OUTPUTS]; + float (*default_pwm_rate)[NUM_MIXER_OUTPUTS]; + float (*Fx)[NUM_MIXER_OUTPUTS]; + float (*Fy)[NUM_MIXER_OUTPUTS]; + float (*Fz)[NUM_MIXER_OUTPUTS]; + float (*Qx)[NUM_MIXER_OUTPUTS]; + float (*Qy)[NUM_MIXER_OUTPUTS]; + float (*Qz)[NUM_MIXER_OUTPUTS]; + } mixer_selection_t; + typedef struct { output_type_t type; @@ -105,162 +120,185 @@ class Mixer : public ParamListenerInterface aux_command_t aux_command_; output_type_t combined_output_type_[NUM_TOTAL_OUTPUTS]; - void write_motor(uint8_t index, float value); - void write_servo(uint8_t index, float value); + void load_primary_mixer_values(); + void load_secondary_mixer_values(); + mixer_t invert_mixer(const mixer_t mixer_to_invert); + float mix_multirotor_with_motor_parameters(Controller::Output commands); + float mix_multirotor_without_motor_parameters(Controller::Output commands); + void select_primary_or_secondary_mixer(); // clang-format off const mixer_t esc_calibration_mixing = { - { M, M, M, M, M, M, NONE, NONE, NONE, NONE}, // output type + { M, M, M, M, M, M, M, M, M, M}, // output type { 50, 50, 50, 50, 50, 50, 50, 50, 50, 50}, // Rate (Hz) - {1.0f, 1.0f, 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, 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 + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F_x Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F_y Mix + {0.1f, 0.1f, 0.1f, 0.1f, 0.1f, 0.1f, 0.1f, 0.1f, 0.1f, 0.1f}, // F_z Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Q_x Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Q_y Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Q_z Mix const mixer_t quadcopter_plus_mixing = { - { M, M, M, M, NONE, NONE, NONE, NONE, NONE, NONE}, // output_type - { 490, 490, 490, 490, 50, 50, 50, 50, 50, 50}, // 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 + {M, M, M, M, AUX, AUX, AUX, AUX, AUX, AUX}, // output type + {490, 490, 490, 490, 50, 50, 50, 50, 50, 50}, // Rate (Hz) + { 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0, 0, 0, 0, 0, 0}, // F_x Mix + { 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0, 0, 0, 0, 0, 0}, // F_y Mix + {-0.2500f, -0.2500f, -0.2500f, -0.2500f, 0, 0, 0, 0, 0, 0}, // F_z Mix + { 0.0000f, -1.0000f, 0.0000f, 1.0000f, 0, 0, 0, 0, 0, 0}, // Q_x Mix + { 1.0000f, 0.0000f, -1.0000f, 0.0000f, 0, 0, 0, 0, 0, 0}, // Q_y Mix + { 1.0000f, -1.0000f, 1.0000f, -1.0000f, 0, 0, 0, 0, 0, 0}}; // Q_z Mix const mixer_t quadcopter_x_mixing = { - { M, M, M, M, NONE, NONE, NONE, NONE, NONE, NONE}, // output_type - { 490, 490, 490, 490, 50, 50, 50, 50, 50, 50}, // 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 + {M, M, M, M, AUX, AUX, AUX, AUX, AUX, AUX}, // output type + {490, 490, 490, 490, 50, 50, 50, 50, 50, 50}, // Rate (Hz) + { 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0, 0, 0, 0, 0, 0}, // F_x Mix + { 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0, 0, 0, 0, 0, 0}, // F_y Mix + {-0.2500f, -0.2500f, -0.2500f, -0.2500f, 0, 0, 0, 0, 0, 0}, // F_z Mix + {-0.7071f, -0.7071f, 0.7071f, 0.7071f, 0, 0, 0, 0, 0, 0}, // Q_x Mix + { 0.7071f, -0.7071f, -0.7071f, 0.7071f, 0, 0, 0, 0, 0, 0}, // Q_y Mix + { 1.0000f, -1.0000f, 1.0000f, -1.0000f, 0, 0, 0, 0, 0, 0}}; // Q_z Mix const mixer_t hex_plus_mixing = { - { M, M, M, M, M, M, NONE, NONE, NONE, NONE}, // output_type - { 490, 490, 490, 490, 490, 490, 50, 50, 50, 50}, // 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 + {M, M, M, M, M, M, AUX, AUX, AUX, AUX}, // output type + {490, 490, 490, 490, 490, 490, 490, 490, 50, 50}, // Rate (Hz) + { 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0, 0, 0, 0}, // F_x Mix + { 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0, 0, 0, 0}, // F_y Mix + {-0.1667f, -0.1667f, -0.1667f, -0.1667f, -0.1667f, -0.1667f, 0, 0, 0, 0}, // F_z Mix + { 0.0000f, -0.8660f, -0.8660f, 0.0000f, 0.8660f, 0.8660f, 0, 0, 0, 0}, // Q_x Mix + { 1.0000f, 0.5000f, -0.5000f, -1.0000f, -0.5000f, 0.5000f, 0, 0, 0, 0}, // Q_y Mix + { 1.0000f, -1.0000f, 1.0000f, -1.0000f, 1.0000f, -1.0000f, 0, 0, 0, 0}}; // Q_z Mix const mixer_t hex_x_mixing = { - { M, M, M, M, M, M, NONE, NONE, NONE, NONE}, // output_type - { 490, 490, 490, 490, 490, 490, 50, 50, 50, 50}, // 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 + {M, M, M, M, M, M, AUX, AUX, AUX, AUX}, // output type + {490, 490, 490, 490, 490, 490, 490, 490, 50, 50}, // Rate (Hz) + { 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0, 0, 0, 0}, // F_x Mix + { 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0, 0, 0, 0}, // F_y Mix + {-0.1667f, -0.1667f, -0.1667f, -0.1667f, -0.1667f, -0.1667f, 0, 0, 0, 0}, // F_z Mix + {-0.5000f, -1.0000f, -0.5000f, 0.5000f, 1.0000f, 0.5000f, 0, 0, 0, 0}, // Q_x Mix + { 0.8660f, 0.0000f, -0.8660f, -0.8660f, 0.0000f, 0.8660f, 0, 0, 0, 0}, // Q_y Mix + { 1.0000f, -1.0000f, 1.0000f, -1.0000f, 1.0000f, -1.0000f, 0, 0, 0, 0}}; // Q_z Mix const mixer_t octocopter_plus_mixing = { - { M, M, M, M, M, M, M, M, NONE, NONE}, // output_type - { 490, 490, 490, 490, 490, 490, 490, 490, 50, 50}, // 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 + {M, M, M, M, M, M, M, M, AUX, AUX}, // output type + {490, 490, 490, 490, 490, 490, 490, 490, 50, 50}, // Rate (Hz) + { 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0, 0}, // F_x Mix + { 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0, 0}, // F_y Mix + {-0.1250f, -0.1250f, -0.1250f, -0.1250f, -0.1250f, -0.1250f, -0.1250f, -0.1250f, 0, 0}, // F_z Mix + { 0.0000f, -0.7071f, -1.0000f, -0.7071f, 0.0000f, 0.7071f, 1.0000f, 0.7071f, 0, 0}, // Q_x Mix + { 1.0000f, 0.7071f, 0.0000f, -0.7071f, -1.0000f, -0.7071f, 0.0000f, 0.7071f, 0, 0}, // Q_y Mix + { 1.0000f, -1.0000f, 1.0000f, -1.0000f, 1.0000f, -1.0000f, 1.0000f, -1.0000f, 0, 0}}; // Q_z Mix const mixer_t octocopter_x_mixing = { - { M, M, M, M, M, M, M, M, NONE, NONE}, // output_type - { 490, 490, 490, 490, 490, 490, 490, 490, 50, 50}, // 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 + {M, M, M, M, M, M, M, M, AUX, AUX}, // output type + {490, 490, 490, 490, 490, 490, 490, 490, 50, 50}, // Rate (Hz) + { 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0, 0}, // F_x Mix + { 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0, 0}, // F_y Mix + {-0.1250f, -0.1250f, -0.1250f, -0.1250f, -0.1250f, -0.1250f, -0.1250f, -0.1250f, 0, 0}, // F_z Mix + {-0.3827f, -0.9239f, -0.9239f, -0.3827f, 0.3827f, 0.9239f, 0.9239f, 0.3827f, 0, 0}, // Q_x Mix + { 0.9239f, 0.3827f, -0.3827f, -0.9239f, -0.9239f, -0.3827f, 0.3827f, 0.9239f, 0, 0}, // Q_y Mix + { 1.0000f, -1.0000f, 1.0000f, -1.0000f, 1.0000f, -1.0000f, 1.0000f, -1.0000f, 0, 0}}; // Q_z Mix const mixer_t Y6_mixing = { - { M, M, M, M, M, M, NONE, NONE, NONE, NONE}, // output_type - { 490, 490, 490, 490, 490, 490, 50, 50, 50, 50}, // 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 + {M, M, M, M, M, M, AUX, AUX, AUX, AUX}, // output type + {490, 490, 490, 490, 490, 490, 490, 490, 50, 50}, // Rate (Hz) + { 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0, 0, 0, 0}, // F_x Mix + { 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0, 0, 0, 0}, // F_y Mix + {-0.1667f, -0.1667f, -0.1667f, -0.1667f, -0.1667f, -0.1667f, 0, 0, 0, 0}, // F_z Mix + {-0.8660f, -0.8660f, 0.0000f, 0.0000f, 0.8660f, 0.8660f, 0, 0, 0, 0}, // Q_x Mix + { 0.5000f, 0.5000f, -1.0000f, -1.0000f, 0.5000f, 0.5000f, 0, 0, 0, 0}, // Q_y Mix + { 1.0000f, -1.0000f, 1.0000f, -1.0000f, 1.0000f, -1.0000f, 0, 0, 0, 0}}; // Q_z Mix const mixer_t X8_mixing = { - { M, M, M, M, M, M, M, M, NONE, NONE}, // output_type - { 490, 490, 490, 490, 490, 490, 490, 490, 50, 50}, // 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, NONE, S, NONE, NONE, NONE, NONE, NONE}, // output_type - { 490, 490, 490, 50, 50, 50, 50, 50, 50, 50}, // Rate (Hz) - { 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 + {M, M, M, M, M, M, M, M, AUX, AUX}, // output type + {490, 490, 490, 490, 490, 490, 490, 490, 50, 50}, // Rate (Hz) + { 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0, 0}, // F_x Mix + { 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0.0000f, 0, 0}, // F_y Mix + {-0.1250f, -0.1250f, -0.1250f, -0.1250f, -0.1250f, -0.1250f, -0.1250f, -0.1250f, 0, 0}, // F_z Mix + {-0.7071f, -0.7071f, -0.7071f, -0.7071f, 0.7071f, 0.7071f, 0.7071f, 0.7071f, 0, 0}, // Q_x Mix + { 0.7071f, 0.7071f, -0.7071f, -0.7071f, -0.7071f, -0.7071f, 0.7071f, 0.7071f, 0, 0}, // Q_y Mix + { 1.0000f, -1.0000f, 1.0000f, -1.0000f, 1.0000f, -1.0000f, 1.0000f, -1.0000f, 0, 0}}; // Q_z Mix const mixer_t fixedwing_mixing = { - { S, S, S, NONE, M, NONE, NONE, NONE, NONE, NONE}, // output type + { S, S, S, AUX, M, AUX, AUX, AUX, AUX, AUX}, // output type { 50, 50, 50, 50, 50, 50, 50, 50, 50, 50}, // Rate (Hz) - {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, 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, NONE, NONE, S, S, M, NONE, NONE, NONE, NONE}, // Ailerons, LRuddervator, RRuddervator, Motor - { 50, 50, 50, 50, 50, 50, 50, 50, 50, 50}, // Rate (Hz) - {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.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.0f, 0.0f, -0.5f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix - {0.0f, 0.0f, 0.0f, 0.5f, 0.5f, 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 - { 50, 50, 50, 50, 490, 490, 490, 490, 50, 50}, // Rate (Hz) - {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}, - { 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 + {0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F_x Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F_y Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F_z Mix + {1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Q_x Mix + {0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Q_y Mix + {0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Q_z Mix + + const mixer_t fixedwing_inverted_vtail_mixing = { + { S, S, S, AUX, M, AUX, AUX, AUX, AUX, AUX}, // Ailerons, LRuddervator, RRuddervator, Motor + { 50, 50, 50, 50, 50, 50, 50, 50, 50, 50}, // Rate (Hz) + {0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F_x Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F_y Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F_z Mix + {1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Q_x Mix + {0.0f, -0.5f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Q_y Mix + {0.0f, 0.5f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Q_z Mix const mixer_t custom_mixing = { - {NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE}, // output type + {AUX, AUX, AUX, AUX, AUX, AUX, AUX, AUX, AUX, AUX}, // output type { 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_; - - const mixer_t* array_of_mixers_[NUM_MIXERS] = { - &esc_calibration_mixing, - &quadcopter_plus_mixing, - &quadcopter_x_mixing, - &hex_plus_mixing, - &hex_x_mixing, - &octocopter_plus_mixing, - &octocopter_x_mixing, - &Y6_mixing, - &X8_mixing, - &tricopter_mixing, - &fixedwing_mixing, - &passthrough_mixing, - &fixedwing_vtail_mixing, - &quadplane_mixing, - &custom_mixing + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F_x Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F_y Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F_z Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Q_x Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Q_y Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Q_z Mix + + mixer_t primary_mixer_; + mixer_t secondary_mixer_; + + mixer_selection_t mixer_to_use_; + bool primary_mixer_is_selected_ = false; + + const mixer_t array_of_mixers_[NUM_MIXERS] = { + esc_calibration_mixing, + quadcopter_plus_mixing, + quadcopter_x_mixing, + hex_plus_mixing, + hex_x_mixing, + octocopter_plus_mixing, + octocopter_x_mixing, + Y6_mixing, + X8_mixing, + fixedwing_mixing, + fixedwing_inverted_vtail_mixing, + custom_mixing, }; // clang-format on + // Define parameters for the mixer + float R_; // Motor resistance + float K_V_; // Motor back-emf constant + float K_Q_; // Motor torque constant + float i_0_; // Motor no-load current + float D_; // Propeller diameter + float C_T_; // Thrust coefficient + float C_Q_; // Torque coefficient + int num_motors_; // Number of motors + float V_max_; // Maximum battery voltage + float l_[NUM_MIXER_OUTPUTS]; // Radial distance from center of mass to motor + float psi_[NUM_MIXER_OUTPUTS]; // Angle of motor from body x-axis + public: Mixer(ROSflight & _rf); void init(); void init_PWM(); void init_mixing(); + void update_parameters(); void mix_output(); void param_change_callback(uint16_t param_id) override; void set_new_aux_command(aux_command_t new_aux_command); inline const float * get_outputs() const { return raw_outputs_; } + + void calculate_mixer_values(); + void mix_multirotor(); + void mix_fixedwing(); }; } // namespace rosflight_firmware diff --git a/include/param.h b/include/param.h index 03d42067..3a082b93 100644 --- a/include/param.h +++ b/include/param.h @@ -62,6 +62,178 @@ enum : uint16_t PARAM_BAUD_RATE = 0, PARAM_SERIAL_DEVICE, + PARAM_NUM_MOTORS, + PARAM_MOTOR_RESISTANCE, + PARAM_MOTOR_KV, + PARAM_NO_LOAD_CURRENT, + PARAM_PROP_DIAMETER, + PARAM_PROP_CT, + PARAM_PROP_CQ, + PARAM_VOLT_MAX, + PARAM_USE_MOTOR_PARAMETERS, + + PARAM_PRIMARY_MIXER_OUTPUT_0, + PARAM_PRIMARY_MIXER_OUTPUT_1, + PARAM_PRIMARY_MIXER_OUTPUT_2, + PARAM_PRIMARY_MIXER_OUTPUT_3, + PARAM_PRIMARY_MIXER_OUTPUT_4, + PARAM_PRIMARY_MIXER_OUTPUT_5, + PARAM_PRIMARY_MIXER_OUTPUT_6, + PARAM_PRIMARY_MIXER_OUTPUT_7, + PARAM_PRIMARY_MIXER_OUTPUT_8, + PARAM_PRIMARY_MIXER_OUTPUT_9, + + PARAM_PRIMARY_MIXER_PWM_RATE_0, + PARAM_PRIMARY_MIXER_PWM_RATE_1, + PARAM_PRIMARY_MIXER_PWM_RATE_2, + PARAM_PRIMARY_MIXER_PWM_RATE_3, + PARAM_PRIMARY_MIXER_PWM_RATE_4, + PARAM_PRIMARY_MIXER_PWM_RATE_5, + PARAM_PRIMARY_MIXER_PWM_RATE_6, + PARAM_PRIMARY_MIXER_PWM_RATE_7, + PARAM_PRIMARY_MIXER_PWM_RATE_8, + PARAM_PRIMARY_MIXER_PWM_RATE_9, + + PARAM_PRIMARY_MIXER_0_0, + PARAM_PRIMARY_MIXER_1_0, + PARAM_PRIMARY_MIXER_2_0, + PARAM_PRIMARY_MIXER_3_0, + PARAM_PRIMARY_MIXER_4_0, + PARAM_PRIMARY_MIXER_5_0, + + PARAM_PRIMARY_MIXER_0_1, + PARAM_PRIMARY_MIXER_1_1, + PARAM_PRIMARY_MIXER_2_1, + PARAM_PRIMARY_MIXER_3_1, + PARAM_PRIMARY_MIXER_4_1, + PARAM_PRIMARY_MIXER_5_1, + + PARAM_PRIMARY_MIXER_0_2, + PARAM_PRIMARY_MIXER_1_2, + PARAM_PRIMARY_MIXER_2_2, + PARAM_PRIMARY_MIXER_3_2, + PARAM_PRIMARY_MIXER_4_2, + PARAM_PRIMARY_MIXER_5_2, + + PARAM_PRIMARY_MIXER_0_3, + PARAM_PRIMARY_MIXER_1_3, + PARAM_PRIMARY_MIXER_2_3, + PARAM_PRIMARY_MIXER_3_3, + PARAM_PRIMARY_MIXER_4_3, + PARAM_PRIMARY_MIXER_5_3, + + PARAM_PRIMARY_MIXER_0_4, + PARAM_PRIMARY_MIXER_1_4, + PARAM_PRIMARY_MIXER_2_4, + PARAM_PRIMARY_MIXER_3_4, + PARAM_PRIMARY_MIXER_4_4, + PARAM_PRIMARY_MIXER_5_4, + + PARAM_PRIMARY_MIXER_0_5, + PARAM_PRIMARY_MIXER_1_5, + PARAM_PRIMARY_MIXER_2_5, + PARAM_PRIMARY_MIXER_3_5, + PARAM_PRIMARY_MIXER_4_5, + PARAM_PRIMARY_MIXER_5_5, + + PARAM_PRIMARY_MIXER_0_6, + PARAM_PRIMARY_MIXER_1_6, + PARAM_PRIMARY_MIXER_2_6, + PARAM_PRIMARY_MIXER_3_6, + PARAM_PRIMARY_MIXER_4_6, + PARAM_PRIMARY_MIXER_5_6, + + PARAM_PRIMARY_MIXER_0_7, + PARAM_PRIMARY_MIXER_1_7, + PARAM_PRIMARY_MIXER_2_7, + PARAM_PRIMARY_MIXER_3_7, + PARAM_PRIMARY_MIXER_4_7, + PARAM_PRIMARY_MIXER_5_7, + + PARAM_PRIMARY_MIXER_0_8, + PARAM_PRIMARY_MIXER_1_8, + PARAM_PRIMARY_MIXER_2_8, + PARAM_PRIMARY_MIXER_3_8, + PARAM_PRIMARY_MIXER_4_8, + PARAM_PRIMARY_MIXER_5_8, + + PARAM_PRIMARY_MIXER_0_9, + PARAM_PRIMARY_MIXER_1_9, + PARAM_PRIMARY_MIXER_2_9, + PARAM_PRIMARY_MIXER_3_9, + PARAM_PRIMARY_MIXER_4_9, + PARAM_PRIMARY_MIXER_5_9, + + PARAM_SECONDARY_MIXER_0_0, + PARAM_SECONDARY_MIXER_1_0, + PARAM_SECONDARY_MIXER_2_0, + PARAM_SECONDARY_MIXER_3_0, + PARAM_SECONDARY_MIXER_4_0, + PARAM_SECONDARY_MIXER_5_0, + + PARAM_SECONDARY_MIXER_0_1, + PARAM_SECONDARY_MIXER_1_1, + PARAM_SECONDARY_MIXER_2_1, + PARAM_SECONDARY_MIXER_3_1, + PARAM_SECONDARY_MIXER_4_1, + PARAM_SECONDARY_MIXER_5_1, + + PARAM_SECONDARY_MIXER_0_2, + PARAM_SECONDARY_MIXER_1_2, + PARAM_SECONDARY_MIXER_2_2, + PARAM_SECONDARY_MIXER_3_2, + PARAM_SECONDARY_MIXER_4_2, + PARAM_SECONDARY_MIXER_5_2, + + PARAM_SECONDARY_MIXER_0_3, + PARAM_SECONDARY_MIXER_1_3, + PARAM_SECONDARY_MIXER_2_3, + PARAM_SECONDARY_MIXER_3_3, + PARAM_SECONDARY_MIXER_4_3, + PARAM_SECONDARY_MIXER_5_3, + + PARAM_SECONDARY_MIXER_0_4, + PARAM_SECONDARY_MIXER_1_4, + PARAM_SECONDARY_MIXER_2_4, + PARAM_SECONDARY_MIXER_3_4, + PARAM_SECONDARY_MIXER_4_4, + PARAM_SECONDARY_MIXER_5_4, + + PARAM_SECONDARY_MIXER_0_5, + PARAM_SECONDARY_MIXER_1_5, + PARAM_SECONDARY_MIXER_2_5, + PARAM_SECONDARY_MIXER_3_5, + PARAM_SECONDARY_MIXER_4_5, + PARAM_SECONDARY_MIXER_5_5, + + PARAM_SECONDARY_MIXER_0_6, + PARAM_SECONDARY_MIXER_1_6, + PARAM_SECONDARY_MIXER_2_6, + PARAM_SECONDARY_MIXER_3_6, + PARAM_SECONDARY_MIXER_4_6, + PARAM_SECONDARY_MIXER_5_6, + + PARAM_SECONDARY_MIXER_0_7, + PARAM_SECONDARY_MIXER_1_7, + PARAM_SECONDARY_MIXER_2_7, + PARAM_SECONDARY_MIXER_3_7, + PARAM_SECONDARY_MIXER_4_7, + PARAM_SECONDARY_MIXER_5_7, + + PARAM_SECONDARY_MIXER_0_8, + PARAM_SECONDARY_MIXER_1_8, + PARAM_SECONDARY_MIXER_2_8, + PARAM_SECONDARY_MIXER_3_8, + PARAM_SECONDARY_MIXER_4_8, + PARAM_SECONDARY_MIXER_5_8, + + PARAM_SECONDARY_MIXER_0_9, + PARAM_SECONDARY_MIXER_1_9, + PARAM_SECONDARY_MIXER_2_9, + PARAM_SECONDARY_MIXER_3_9, + PARAM_SECONDARY_MIXER_4_9, + PARAM_SECONDARY_MIXER_5_9, + /*****************************/ /*** MAVLINK CONFIGURATION ***/ /*****************************/ @@ -70,8 +242,6 @@ enum : uint16_t /********************************/ /*** CONTROLLER CONFIGURATION ***/ /********************************/ - PARAM_MAX_COMMAND, - PARAM_PID_ROLL_RATE_P, PARAM_PID_ROLL_RATE_I, PARAM_PID_ROLL_RATE_D, @@ -161,6 +331,7 @@ enum : uint16_t PARAM_RC_Y_CHANNEL, PARAM_RC_Z_CHANNEL, PARAM_RC_F_CHANNEL, + PARAM_RC_F_AXIS, PARAM_RC_ATTITUDE_OVERRIDE_CHANNEL, PARAM_RC_THROTTLE_OVERRIDE_CHANNEL, PARAM_RC_ATT_CONTROL_TYPE_CHANNEL, @@ -175,6 +346,7 @@ enum : uint16_t PARAM_RC_OVERRIDE_DEVIATION, PARAM_OVERRIDE_LAG_TIME, PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, + PARAM_RC_MAX_THROTTLE, PARAM_RC_ATTITUDE_MODE, PARAM_RC_MAX_ROLL, @@ -186,7 +358,8 @@ enum : uint16_t /***************************/ /*** FRAME CONFIGURATION ***/ /***************************/ - PARAM_MIXER, + PARAM_PRIMARY_MIXER, + PARAM_SECONDARY_MIXER, PARAM_FIXED_WING, PARAM_ELEVATOR_REVERSE, diff --git a/include/sensors.h b/include/sensors.h index 6f41f1e4..526fef4c 100644 --- a/include/sensors.h +++ b/include/sensors.h @@ -177,6 +177,7 @@ class Sensors : public ParamListenerInterface Sensors(ROSflight & rosflight); inline const Data & data() const { return data_; } + inline float rho() { return rho_; } void get_filtered_IMU(turbomath::Vector & accel, turbomath::Vector & gyro, uint64_t & stamp_us); // function declarations @@ -227,6 +228,8 @@ class Sensors : public ParamListenerInterface float accel_[3] = {0, 0, 0}; float gyro_[3] = {0, 0, 0}; + float rho_ = 1.225; + bool calibrating_acc_flag_ = false; bool calibrating_gyro_flag_ = false; uint8_t next_sensor_to_update_ = BAROMETER; diff --git a/scripts/param_parser.py b/scripts/param_parser.py index 0e6f7762..189c33cb 100755 --- a/scripts/param_parser.py +++ b/scripts/param_parser.py @@ -19,18 +19,18 @@ i = 0 for line in lines: # search for init_param lines - match = re.search("^\s*init_param", line) + match = re.search(r"^\s*init_param", line) if match != None: - name_with_quotes = re.search("\".*\"", line).group(0) - name = re.search("\w{1,16}", name_with_quotes).group(0) + name_with_quotes = re.search(r"\".*\"", line).group(0) + name = re.search(r"\w{1,16}", name_with_quotes).group(0) if name != 'DEFAULT': params.append(dict()) - params[i]['type'] = re.split("\(", re.split("_", line)[2])[0] + params[i]['type'] = re.split(r"\(", re.split(r"_", line)[2])[0] params[i]['name'] = name # Find default value - params[i]['default'] = re.split("\)", re.split(",", line)[2])[0] + params[i]['default'] = re.split(r"\)", re.split(r",", line)[2])[0] # isolate the comment portion of the line and split it on the "|" characters - comment = re.split("\|", re.split("//", line)[1]) + comment = re.split(r"\|", re.split(r"//", line)[1]) # Isolate the Description params[i]["description"] = comment[0].strip() params[i]["min"] = comment[1].strip() diff --git a/src/comm_manager.cpp b/src/comm_manager.cpp index 6e872c5e..aad134f6 100644 --- a/src/comm_manager.cpp +++ b/src/comm_manager.cpp @@ -237,36 +237,46 @@ void CommManager::offboard_control_callback(const CommLinkInterface::OffboardCon { // 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; + new_offboard_command.Qx.value = control.Qx.value; + new_offboard_command.Qy.value = control.Qy.value; + new_offboard_command.Qz.value = control.Qz.value; + new_offboard_command.Fx.value = control.Fx.value; + new_offboard_command.Fy.value = control.Fy.value; + new_offboard_command.Fz.value = control.Fz.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; + new_offboard_command.Qx.active = control.Qx.valid; + new_offboard_command.Qy.active = control.Qy.valid; + new_offboard_command.Qz.active = control.Qz.valid; + new_offboard_command.Fx.active = control.Fx.valid; + new_offboard_command.Fy.active = control.Fy.valid; + new_offboard_command.Fz.active = control.Fz.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; + new_offboard_command.Qx.type = PASSTHROUGH; + new_offboard_command.Qy.type = PASSTHROUGH; + new_offboard_command.Qz.type = PASSTHROUGH; + new_offboard_command.Fx.type = PASSTHROUGH; + new_offboard_command.Fy.type = PASSTHROUGH; + new_offboard_command.Fz.type = PASSTHROUGH; 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; + new_offboard_command.Qx.type = RATE; + new_offboard_command.Qy.type = RATE; + new_offboard_command.Qz.type = RATE; + new_offboard_command.Fx.type = THROTTLE; + new_offboard_command.Fy.type = THROTTLE; + new_offboard_command.Fz.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; + new_offboard_command.Qx.type = ANGLE; + new_offboard_command.Qy.type = ANGLE; + new_offboard_command.Qz.type = RATE; + new_offboard_command.Fx.type = THROTTLE; + new_offboard_command.Fy.type = THROTTLE; + new_offboard_command.Fz.type = THROTTLE; break; } @@ -283,7 +293,7 @@ void CommManager::aux_command_callback(const CommLinkInterface::AuxCommand & com 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].type = Mixer::AUX; new_aux_command.channel[i].value = 0; break; case CommLinkInterface::AuxCommand::Type::SERVO: @@ -350,9 +360,10 @@ void CommManager::send_status(void) if (!initialized_) { return; } uint8_t control_mode = 0; - if (RF_.params_.get_param_int(PARAM_FIXED_WING) || RF_.command_manager_.combined_control().x.type == PASSTHROUGH) { + if (RF_.params_.get_param_int(PARAM_FIXED_WING) + || RF_.command_manager_.combined_control().Qx.type == PASSTHROUGH) { control_mode = MODE_PASS_THROUGH; - } else if (RF_.command_manager_.combined_control().x.type == ANGLE) { + } else if (RF_.command_manager_.combined_control().Qx.type == ANGLE) { control_mode = MODE_ROLL_PITCH_YAWRATE_THROTTLE; } else { control_mode = MODE_ROLLRATE_PITCHRATE_YAWRATE_THROTTLE; diff --git a/src/command_manager.cpp b/src/command_manager.cpp index a66aaa64..ed1bc4ff 100644 --- a/src/command_manager.cpp +++ b/src/command_manager.cpp @@ -91,7 +91,23 @@ void CommandManager::init_failsafe() RF_.state_manager_.clear_error(StateManager::ERROR_INVALID_FAILSAFE); } - multirotor_failsafe_command_.F.value = failsafe_thr_param; + // Make sure the failsafe is set to the axis associated with the RC F command + switch (static_cast(RF_.params_.get_param_int(PARAM_RC_F_AXIS))) { + case X_AXIS: + multirotor_failsafe_command_.Fx.value = failsafe_thr_param; + break; + case Y_AXIS: + multirotor_failsafe_command_.Fy.value = failsafe_thr_param; + break; + case Z_AXIS: + multirotor_failsafe_command_.Fz.value = failsafe_thr_param; + break; + default: + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, + "Invalid RC F axis. Defaulting to z-axis."); + multirotor_failsafe_command_.Fz.value = failsafe_thr_param; + break; + } if (fixedwing) { failsafe_command_ = fixedwing_failsafe_command_; @@ -103,17 +119,44 @@ void CommandManager::init_failsafe() 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); + rc_command_.Qx.value = RF_.rc_.stick(RC::STICK_X); + rc_command_.Qy.value = RF_.rc_.stick(RC::STICK_Y); + rc_command_.Qz.value = RF_.rc_.stick(RC::STICK_Z); + + // Load the RC command based on the axis associated with the RC F command + switch (static_cast(RF_.params_.get_param_int(PARAM_RC_F_AXIS))) { + case X_AXIS: // RC F = X axis + rc_command_.Fx.value = RF_.rc_.stick(RC::STICK_F); + rc_command_.Fy.value = 0.0; + rc_command_.Fz.value = 0.0; + break; + case Y_AXIS: // RC F = Y axis + rc_command_.Fx.value = 0.0; + rc_command_.Fy.value = RF_.rc_.stick(RC::STICK_F); + rc_command_.Fz.value = 0.0; + break; + case Z_AXIS: + rc_command_.Fx.value = 0.0; + rc_command_.Fy.value = 0.0; + rc_command_.Fz.value = RF_.rc_.stick(RC::STICK_F); + break; + default: + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, + "Invalid RC F axis. Defaulting to z-axis."); + rc_command_.Fx.value = 0.0; + rc_command_.Fy.value = 0.0; + rc_command_.Fz.value = RF_.rc_.stick(RC::STICK_F); + break; + } // 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; + rc_command_.Qx.type = PASSTHROUGH; + rc_command_.Qy.type = PASSTHROUGH; + rc_command_.Qz.type = PASSTHROUGH; + rc_command_.Fx.type = PASSTHROUGH; + rc_command_.Fy.type = PASSTHROUGH; + rc_command_.Fz.type = PASSTHROUGH; } else { // roll and pitch control_type_t roll_pitch_type; @@ -124,28 +167,30 @@ void CommandManager::interpret_rc(void) (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; + rc_command_.Qx.type = roll_pitch_type; + rc_command_.Qy.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); + rc_command_.Qx.value *= RF_.params_.get_param_float(PARAM_RC_MAX_ROLLRATE); + rc_command_.Qy.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); + rc_command_.Qx.value *= RF_.params_.get_param_float(PARAM_RC_MAX_ROLL); + rc_command_.Qy.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); + rc_command_.Qz.type = RATE; + rc_command_.Qz.value *= RF_.params_.get_param_float(PARAM_RC_MAX_YAWRATE); // throttle - rc_command_.F.type = THROTTLE; + rc_command_.Fx.type = THROTTLE; + rc_command_.Fy.type = THROTTLE; + rc_command_.Fz.type = THROTTLE; } } @@ -190,15 +235,34 @@ bool CommandManager::do_roll_pitch_yaw_muxing(MuxChannel channel) bool CommandManager::do_throttle_muxing(void) { bool override_this_channel = false; + MuxChannel selected_channel; + // Determine which channel to check based on which axis the RC channel corresponds to + switch (static_cast(RF_.params_.get_param_int(PARAM_RC_F_AXIS))) { + case X_AXIS: + selected_channel = MUX_FX; + break; + case Y_AXIS: + selected_channel = MUX_FY; + break; + case Z_AXIS: + selected_channel = MUX_FZ; + break; + default: + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, + "Invalid RC F axis. Defaulting to z-axis."); + selected_channel = MUX_FZ; + break; + } + // 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) { + if (muxes[selected_channel].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); + override_this_channel = (muxes[selected_channel].rc->value < muxes[selected_channel].onboard->value); } else { override_this_channel = false; } @@ -208,11 +272,15 @@ bool CommandManager::do_throttle_muxing(void) } // 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; + *muxes[selected_channel].combined = override_this_channel ? *muxes[selected_channel].rc : *muxes[selected_channel].onboard; return override_this_channel; } -bool CommandManager::rc_override_active() { return rc_override_; } +bool CommandManager::rc_override_active() { return rc_throttle_override_ || rc_attitude_override_; } + +bool CommandManager::rc_throttle_override_active() { return rc_throttle_override_; } + +bool CommandManager::rc_attitude_override_active() { return rc_attitude_override_; } bool CommandManager::offboard_control_active() { @@ -242,7 +310,7 @@ void CommandManager::override_combined_command_with_rc() bool CommandManager::run() { - bool last_rc_override = rc_override_; + bool last_rc_override = rc_override_active(); // Check for and apply failsafe command if (RF_.state_manager_.state().failsafe) { @@ -255,20 +323,22 @@ bool CommandManager::run() 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; + offboard_command_.Fx.active = false; + offboard_command_.Fy.active = false; + offboard_command_.Fz.active = false; + offboard_command_.Qx.active = false; + offboard_command_.Qy.active = false; + offboard_command_.Qz.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(); + rc_attitude_override_ = do_roll_pitch_yaw_muxing(MUX_QX); + rc_attitude_override_ |= do_roll_pitch_yaw_muxing(MUX_QY); + rc_attitude_override_ |= do_roll_pitch_yaw_muxing(MUX_QZ); + rc_throttle_override_ = do_throttle_muxing(); // Light to indicate override - if (rc_override_) { + if (rc_override_active()) { RF_.board_.led0_on(); } else { RF_.board_.led0_off(); @@ -276,7 +346,7 @@ bool CommandManager::run() } // There was a change in rc_override state - if (last_rc_override != rc_override_) { RF_.comm_manager_.update_status(); } + if (last_rc_override != rc_override_active()) { RF_.comm_manager_.update_status(); } return true; } diff --git a/src/controller.cpp b/src/controller.cpp index 05e6c16f..a2f8c584 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -49,25 +49,62 @@ void Controller::init() { prev_time_us_ = 0; - float max = RF_.params_.get_param_float(PARAM_MAX_COMMAND); - float min = -max; + // Calculate the max thrust to convert from throttle setting to thrust + calculate_max_thrust(); + + // Don't saturate the torque values + float max_torque = INFINITY; + float min_torque = -max_torque; + 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); + RF_.params_.get_param_float(PARAM_PID_ROLL_ANGLE_D), max_torque, min_torque, 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); + RF_.params_.get_param_float(PARAM_PID_ROLL_RATE_D), max_torque, min_torque, 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); + RF_.params_.get_param_float(PARAM_PID_PITCH_ANGLE_D), max_torque, min_torque, 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); + RF_.params_.get_param_float(PARAM_PID_PITCH_RATE_D), max_torque, min_torque, 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); + RF_.params_.get_param_float(PARAM_PID_YAW_RATE_D), max_torque, min_torque, tau); +} + +void Controller::calculate_max_thrust() +{ + float R = RF_.params_.get_param_float(PARAM_MOTOR_RESISTANCE); + float D = RF_.params_.get_param_float(PARAM_PROP_DIAMETER); + float rho = RF_.sensors_.rho(); + float CQ = RF_.params_.get_param_float(PARAM_PROP_CQ); + float CT = RF_.params_.get_param_float(PARAM_PROP_CT); + float KV = RF_.params_.get_param_float(PARAM_MOTOR_KV); + float i0 = RF_.params_.get_param_float(PARAM_NO_LOAD_CURRENT); + float KQ = KV; + int num_motors = RF_.params_.get_param_int(PARAM_NUM_MOTORS); + + float V_max = RF_.params_.get_param_float(PARAM_VOLT_MAX); + float a = R * rho * pow(D, 5.0) * CQ / (4 * pow(M_PI, 2.0) * KQ); + float b = KV; + float c = i0 * R - V_max; + + // Using Eq. 4.19 and setting equal to the equation at the beginning of 4.3 in Small Unmanned Aircraft + // We only need the positive root. Since a and b positive, sqrt term must be positive + float omega = (-b + sqrt(pow(b, 2.0) - 4 * a * c)) / (2 * a); + + // Calculate the max thrust from Eq in 4.3 of Small Unmanned Aircraft + // Note that the equation is for a single motor, so to calculate max thrust, we need to multiply by the number of motors + max_thrust_ = rho * pow(D, 4.0) * CT * pow(omega, 2.0) / (4 * pow(M_PI, 2.0)) * num_motors; +} + +bool Controller::is_throttle_high(float threshold) { + return RF_.command_manager_.combined_control().Fx.value > threshold || + RF_.command_manager_.combined_control().Fy.value > threshold || + RF_.command_manager_.combined_control().Fz.value > threshold; } void Controller::run() @@ -86,23 +123,26 @@ void Controller::run() 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; + && is_throttle_high(0.1f) && dt_us < 10000; // Run the PID loops - turbomath::Vector pid_output = run_pid_loops( + Controller::Output 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; + output_.Qx = pid_output.Qx + RF_.params_.get_param_float(PARAM_X_EQ_TORQUE); + output_.Qy = pid_output.Qy + RF_.params_.get_param_float(PARAM_Y_EQ_TORQUE); + output_.Qz = pid_output.Qz + RF_.params_.get_param_float(PARAM_Z_EQ_TORQUE); + + output_.Fx = pid_output.Fx; + output_.Fy = pid_output.Fy; + output_.Fz = pid_output.Fz; } void Controller::calculate_equilbrium_torque_from_rc() { + // TODO: Verify that this is working! // Make sure we are disarmed if (!(RF_.state_manager_.state().armed)) { // Tell the user that we are doing a equilibrium torque calibration @@ -129,16 +169,16 @@ void Controller::calculate_equilbrium_torque_from_rc() // 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 = + Controller::Output 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)); + pid_output.Qx + 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)); + pid_output.Qy + 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)); + pid_output.Qz + RF_.params_.get_param_float(PARAM_Z_EQ_TORQUE)); RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, "Equilibrium torques found and applied."); @@ -168,7 +208,14 @@ void Controller::param_change_callback(uint16_t param_id) case PARAM_PID_YAW_RATE_P: case PARAM_PID_YAW_RATE_I: case PARAM_PID_YAW_RATE_D: - case PARAM_MAX_COMMAND: + case PARAM_MOTOR_RESISTANCE: + case PARAM_PROP_DIAMETER: + case PARAM_PROP_CT: + case PARAM_MOTOR_KV: + case PARAM_NO_LOAD_CURRENT: + case PARAM_VOLT_MAX: + case PARAM_NUM_MOTORS: + case PARAM_RC_MAX_THROTTLE: case PARAM_PID_TAU: init(); break; @@ -178,39 +225,84 @@ void Controller::param_change_callback(uint16_t param_id) } } -turbomath::Vector Controller::run_pid_loops(uint32_t dt_us, const Estimator::State & state, - const control_t & command, bool update_integrators) +Controller::Output 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; + Controller::Output 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); + if (command.Qx.type == RATE) { + out.Qx = roll_rate_.run(dt, state.angular_velocity.x, command.Qx.value, update_integrators); + } else if (command.Qx.type == ANGLE) { + out.Qx = + roll_.run(dt, state.roll, command.Qx.value, update_integrators, state.angular_velocity.x); } else { - out.x = command.x.value; + out.Qx = command.Qx.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); + if (command.Qy.type == RATE) { + out.Qy = pitch_rate_.run(dt, state.angular_velocity.y, command.Qy.value, update_integrators); + } else if (command.Qy.type == ANGLE) { + out.Qy = + pitch_.run(dt, state.pitch, command.Qy.value, update_integrators, state.angular_velocity.y); } else { - out.y = command.y.value; + out.Qy = command.Qy.value; } // YAW - if (command.z.type == RATE) { - out.z = yaw_rate_.run(dt, state.angular_velocity.z, command.z.value, update_integrators); + if (command.Qz.type == RATE) { + out.Qz = yaw_rate_.run(dt, state.angular_velocity.z, command.Qz.value, update_integrators); + } else { + out.Qz = command.Qz.value; + } + + // Fx + if (command.Fx.type == THROTTLE) { + // Scales the saturation limit by RC_MAX_THROTTLE to maintain controllability + // during aggressive maneuvers. + out.Fx = command.Fx.value * RF_.params_.get_param_float(PARAM_RC_MAX_THROTTLE); + + if (RF_.params_.get_param_int(PARAM_USE_MOTOR_PARAMETERS)) { + out.Fx *= max_thrust_; + } + } else { + // If it is not a throttle setting then pass directly to the mixer. + out.Fx = command.Fx.value; + } + + // Fy + if (command.Fy.type == THROTTLE) { + // Scales the saturation limit by RC_MAX_THROTTLE to maintain controllability + // during aggressive maneuvers. + out.Fy = command.Fy.value * RF_.params_.get_param_float(PARAM_RC_MAX_THROTTLE); + + if (RF_.params_.get_param_int(PARAM_USE_MOTOR_PARAMETERS)) { + out.Fy *= max_thrust_; + } + } else { + // If it is not a throttle setting then pass directly to the mixer. + out.Fy = command.Fy.value; + } + + // Fz + if (command.Fz.type == THROTTLE) { + // Scales the saturation limit by RC_MAX_THROTTLE to maintain controllability + // during aggressive maneuvers. + // Also note the negative sign. Since the mixer assumes the inputs are in the NED + // frame, a throttle command corresponds to a thrust command in the negative direction. + // Note that this also assumes that a high throttle means fly "up" (negative down) + out.Fz = -command.Fz.value * RF_.params_.get_param_float(PARAM_RC_MAX_THROTTLE); + + if (RF_.params_.get_param_int(PARAM_USE_MOTOR_PARAMETERS)) { + out.Fz *= max_thrust_; + } } else { - out.z = command.z.value; + // If it is not a throttle setting then pass directly to the mixer. + out.Fz = command.Fz.value; } return out; @@ -244,7 +336,7 @@ float Controller::PID::run(float dt, float x, float x_c, bool update_integrator) 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) + // See Eq. 10.4 of Introduction to Feedback Control by Beard, McLain, Peterson, Killpack differentiator_ = (2.0f * tau_ - dt) / (2.0f * tau_ + dt) * differentiator_ + 2.0f / (2.0f * tau_ + dt) * (x - prev_x_); xdot = differentiator_; @@ -281,7 +373,6 @@ float Controller::PID::run(float dt, float x, float x_c, bool update_integrator, 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_; diff --git a/src/mixer.cpp b/src/mixer.cpp index 14f57821..b50342f1 100644 --- a/src/mixer.cpp +++ b/src/mixer.cpp @@ -40,7 +40,10 @@ namespace rosflight_firmware Mixer::Mixer(ROSflight & _rf) : RF_(_rf) { - mixer_to_use_ = nullptr; + for (int8_t i = 0; i < NUM_TOTAL_OUTPUTS; i++) { + aux_command_.channel[i].type = AUX; + aux_command_.channel[i].value = 0.0f; + } } void Mixer::init() { init_mixing(); } @@ -48,9 +51,162 @@ void Mixer::init() { init_mixing(); } void Mixer::param_change_callback(uint16_t param_id) { switch (param_id) { - case PARAM_MIXER: + case PARAM_PRIMARY_MIXER_OUTPUT_0: + case PARAM_PRIMARY_MIXER_OUTPUT_1: + case PARAM_PRIMARY_MIXER_OUTPUT_2: + case PARAM_PRIMARY_MIXER_OUTPUT_3: + case PARAM_PRIMARY_MIXER_OUTPUT_4: + case PARAM_PRIMARY_MIXER_OUTPUT_5: + case PARAM_PRIMARY_MIXER_OUTPUT_6: + case PARAM_PRIMARY_MIXER_OUTPUT_7: + case PARAM_PRIMARY_MIXER_OUTPUT_8: + case PARAM_PRIMARY_MIXER_OUTPUT_9: + case PARAM_PRIMARY_MIXER_PWM_RATE_0: + case PARAM_PRIMARY_MIXER_PWM_RATE_1: + case PARAM_PRIMARY_MIXER_PWM_RATE_2: + case PARAM_PRIMARY_MIXER_PWM_RATE_3: + case PARAM_PRIMARY_MIXER_PWM_RATE_4: + case PARAM_PRIMARY_MIXER_PWM_RATE_5: + case PARAM_PRIMARY_MIXER_PWM_RATE_6: + case PARAM_PRIMARY_MIXER_PWM_RATE_7: + case PARAM_PRIMARY_MIXER_PWM_RATE_8: + case PARAM_PRIMARY_MIXER_PWM_RATE_9: + case PARAM_PRIMARY_MIXER_0_0: + case PARAM_PRIMARY_MIXER_1_0: + case PARAM_PRIMARY_MIXER_2_0: + case PARAM_PRIMARY_MIXER_3_0: + case PARAM_PRIMARY_MIXER_4_0: + case PARAM_PRIMARY_MIXER_5_0: + case PARAM_PRIMARY_MIXER_0_1: + case PARAM_PRIMARY_MIXER_1_1: + case PARAM_PRIMARY_MIXER_2_1: + case PARAM_PRIMARY_MIXER_3_1: + case PARAM_PRIMARY_MIXER_4_1: + case PARAM_PRIMARY_MIXER_5_1: + case PARAM_PRIMARY_MIXER_0_2: + case PARAM_PRIMARY_MIXER_1_2: + case PARAM_PRIMARY_MIXER_2_2: + case PARAM_PRIMARY_MIXER_3_2: + case PARAM_PRIMARY_MIXER_4_2: + case PARAM_PRIMARY_MIXER_5_2: + case PARAM_PRIMARY_MIXER_0_3: + case PARAM_PRIMARY_MIXER_1_3: + case PARAM_PRIMARY_MIXER_2_3: + case PARAM_PRIMARY_MIXER_3_3: + case PARAM_PRIMARY_MIXER_4_3: + case PARAM_PRIMARY_MIXER_5_3: + case PARAM_PRIMARY_MIXER_0_4: + case PARAM_PRIMARY_MIXER_1_4: + case PARAM_PRIMARY_MIXER_2_4: + case PARAM_PRIMARY_MIXER_3_4: + case PARAM_PRIMARY_MIXER_4_4: + case PARAM_PRIMARY_MIXER_5_4: + case PARAM_PRIMARY_MIXER_0_5: + case PARAM_PRIMARY_MIXER_1_5: + case PARAM_PRIMARY_MIXER_2_5: + case PARAM_PRIMARY_MIXER_3_5: + case PARAM_PRIMARY_MIXER_4_5: + case PARAM_PRIMARY_MIXER_5_5: + case PARAM_PRIMARY_MIXER_0_6: + case PARAM_PRIMARY_MIXER_1_6: + case PARAM_PRIMARY_MIXER_2_6: + case PARAM_PRIMARY_MIXER_3_6: + case PARAM_PRIMARY_MIXER_4_6: + case PARAM_PRIMARY_MIXER_5_6: + case PARAM_PRIMARY_MIXER_0_7: + case PARAM_PRIMARY_MIXER_1_7: + case PARAM_PRIMARY_MIXER_2_7: + case PARAM_PRIMARY_MIXER_3_7: + case PARAM_PRIMARY_MIXER_4_7: + case PARAM_PRIMARY_MIXER_5_7: + case PARAM_PRIMARY_MIXER_0_8: + case PARAM_PRIMARY_MIXER_1_8: + case PARAM_PRIMARY_MIXER_2_8: + case PARAM_PRIMARY_MIXER_3_8: + case PARAM_PRIMARY_MIXER_4_8: + case PARAM_PRIMARY_MIXER_5_8: + case PARAM_PRIMARY_MIXER_0_9: + case PARAM_PRIMARY_MIXER_1_9: + case PARAM_PRIMARY_MIXER_2_9: + case PARAM_PRIMARY_MIXER_3_9: + case PARAM_PRIMARY_MIXER_4_9: + case PARAM_PRIMARY_MIXER_5_9: + + case PARAM_SECONDARY_MIXER_0_0: + case PARAM_SECONDARY_MIXER_1_0: + case PARAM_SECONDARY_MIXER_2_0: + case PARAM_SECONDARY_MIXER_3_0: + case PARAM_SECONDARY_MIXER_4_0: + case PARAM_SECONDARY_MIXER_5_0: + case PARAM_SECONDARY_MIXER_0_1: + case PARAM_SECONDARY_MIXER_1_1: + case PARAM_SECONDARY_MIXER_2_1: + case PARAM_SECONDARY_MIXER_3_1: + case PARAM_SECONDARY_MIXER_4_1: + case PARAM_SECONDARY_MIXER_5_1: + case PARAM_SECONDARY_MIXER_0_2: + case PARAM_SECONDARY_MIXER_1_2: + case PARAM_SECONDARY_MIXER_2_2: + case PARAM_SECONDARY_MIXER_3_2: + case PARAM_SECONDARY_MIXER_4_2: + case PARAM_SECONDARY_MIXER_5_2: + case PARAM_SECONDARY_MIXER_0_3: + case PARAM_SECONDARY_MIXER_1_3: + case PARAM_SECONDARY_MIXER_2_3: + case PARAM_SECONDARY_MIXER_3_3: + case PARAM_SECONDARY_MIXER_4_3: + case PARAM_SECONDARY_MIXER_5_3: + case PARAM_SECONDARY_MIXER_0_4: + case PARAM_SECONDARY_MIXER_1_4: + case PARAM_SECONDARY_MIXER_2_4: + case PARAM_SECONDARY_MIXER_3_4: + case PARAM_SECONDARY_MIXER_4_4: + case PARAM_SECONDARY_MIXER_5_4: + case PARAM_SECONDARY_MIXER_0_5: + case PARAM_SECONDARY_MIXER_1_5: + case PARAM_SECONDARY_MIXER_2_5: + case PARAM_SECONDARY_MIXER_3_5: + case PARAM_SECONDARY_MIXER_4_5: + case PARAM_SECONDARY_MIXER_5_5: + case PARAM_SECONDARY_MIXER_0_6: + case PARAM_SECONDARY_MIXER_1_6: + case PARAM_SECONDARY_MIXER_2_6: + case PARAM_SECONDARY_MIXER_3_6: + case PARAM_SECONDARY_MIXER_4_6: + case PARAM_SECONDARY_MIXER_5_6: + case PARAM_SECONDARY_MIXER_0_7: + case PARAM_SECONDARY_MIXER_1_7: + case PARAM_SECONDARY_MIXER_2_7: + case PARAM_SECONDARY_MIXER_3_7: + case PARAM_SECONDARY_MIXER_4_7: + case PARAM_SECONDARY_MIXER_5_7: + case PARAM_SECONDARY_MIXER_0_8: + case PARAM_SECONDARY_MIXER_1_8: + case PARAM_SECONDARY_MIXER_2_8: + case PARAM_SECONDARY_MIXER_3_8: + case PARAM_SECONDARY_MIXER_4_8: + case PARAM_SECONDARY_MIXER_5_8: + case PARAM_SECONDARY_MIXER_0_9: + case PARAM_SECONDARY_MIXER_1_9: + case PARAM_SECONDARY_MIXER_2_9: + case PARAM_SECONDARY_MIXER_3_9: + case PARAM_SECONDARY_MIXER_4_9: + case PARAM_SECONDARY_MIXER_5_9: + + case PARAM_PRIMARY_MIXER: + case PARAM_SECONDARY_MIXER: init_mixing(); break; + case PARAM_MOTOR_RESISTANCE: + case PARAM_MOTOR_KV: + case PARAM_NO_LOAD_CURRENT: + case PARAM_PROP_DIAMETER: + case PARAM_PROP_CT: + case PARAM_PROP_CQ: + case PARAM_NUM_MOTORS: + case PARAM_VOLT_MAX: + update_parameters(); + break; case PARAM_MOTOR_PWM_SEND_RATE: case PARAM_RC_TYPE: init_PWM(); @@ -66,16 +222,78 @@ 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); + // Set up the primary mixer, used by the RC safety pilot + mixer_type_t mixer_choice = static_cast(RF_.params_.get_param_int(PARAM_PRIMARY_MIXER)); if (mixer_choice >= NUM_MIXERS) { - RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Invalid Mixer Choice"); + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Invalid mixer choice for primary mixer"); // set the invalid mixer flag RF_.state_manager_.set_error(StateManager::ERROR_INVALID_MIXER); - mixer_to_use_ = nullptr; + primary_mixer_is_selected_ = false; } else { - mixer_to_use_ = array_of_mixers_[mixer_choice]; + if (mixer_choice == CUSTOM) { + // Load the custom mixer for the primary mixer based off the saved parameters. + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO, + "Loading saved custom values to primary mixer..."); + + load_primary_mixer_values(); + + // Update the motor parameters that will be used + update_parameters(); + } else if (mixer_choice != FIXEDWING || + mixer_choice != INVERTED_VTAIL) { + // Invert the selected "canned" matrix + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO, + "Inverting selected mixing matrix..."); + primary_mixer_ = invert_mixer(array_of_mixers_[mixer_choice]); + } else { + // Don't invert the fixedwing mixers + primary_mixer_ = array_of_mixers_[mixer_choice]; + } + + // Load the primary mixer header to the mixer_to_use_ header. Note that both the primary and + // secondary mixers will use the header for the primary mixer + mixer_to_use_.output_type = &primary_mixer_.output_type; + mixer_to_use_.default_pwm_rate = &primary_mixer_.default_pwm_rate; + + // Load the primary mixing values into the mixer_to_use_ by default + mixer_to_use_.Fx = &primary_mixer_.Fx; + mixer_to_use_.Fy = &primary_mixer_.Fy; + mixer_to_use_.Fz = &primary_mixer_.Fz; + mixer_to_use_.Qx = &primary_mixer_.Qx; + mixer_to_use_.Qx = &primary_mixer_.Qx; + mixer_to_use_.Qx = &primary_mixer_.Qx; + + primary_mixer_is_selected_ = true; + } + + // Compute or load the secondary mixer, used by the offboard control + mixer_choice = static_cast(RF_.params_.get_param_int(PARAM_SECONDARY_MIXER)); + + if (mixer_choice >= NUM_MIXERS) { + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO, + "Invalid mixer selected for secondary mixer!"); + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO, + "Secondary mixer defaulting to primary!"); + + secondary_mixer_ = primary_mixer_; + } else if (mixer_choice == CUSTOM) { + // Load the custom mixer for the secondary mixer based off the saved parameters. + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO, + "Loading saved custom values to secondary mixer"); + + load_secondary_mixer_values(); + } else if (mixer_choice != FIXEDWING || + mixer_choice != INVERTED_VTAIL) { + // Invert the selected "canned" matrix + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO, + "Inverting selected mixing matrix..."); + + secondary_mixer_ = invert_mixer(array_of_mixers_[mixer_choice]); + } else { + // Don't invert the fixedwing mixers + secondary_mixer_ = array_of_mixers_[mixer_choice]; } init_PWM(); @@ -86,42 +304,131 @@ void Mixer::init_mixing() } } -void Mixer::init_PWM() +void Mixer::update_parameters() { - if (mixer_to_use_ != nullptr) { - RF_.board_.pwm_init_multi(mixer_to_use_->default_pwm_rate, NUM_MIXER_OUTPUTS); - } else { - RF_.board_.pwm_init_multi(passthrough_mixing.default_pwm_rate, NUM_MIXER_OUTPUTS); + R_ = RF_.params_.get_param_float(PARAM_MOTOR_RESISTANCE); + K_V_ = RF_.params_.get_param_float(PARAM_MOTOR_KV); + K_Q_ = K_V_; + i_0_ = RF_.params_.get_param_float(PARAM_NO_LOAD_CURRENT); + D_ = RF_.params_.get_param_float(PARAM_PROP_DIAMETER); + C_T_ = RF_.params_.get_param_float(PARAM_PROP_CT); + C_Q_ = RF_.params_.get_param_float(PARAM_PROP_CQ); + num_motors_ = RF_.params_.get_param_int(PARAM_NUM_MOTORS); + V_max_ = RF_.params_.get_param_float(PARAM_VOLT_MAX); +} + +Mixer::mixer_t Mixer::invert_mixer(const mixer_t mixer_to_invert) +{ + Eigen::Matrix mixer_matrix; + mixer_matrix.setZero(); + + // Convert the mixer_t to an Eigen matrix + for (int i=0; i> svd( + mixer_matrix, + Eigen::FullPivHouseholderQRPreconditioner | Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::Matrix Sig; + Sig.setZero(); + + // Avoid dividing by zero in the Sigma matrix + if (svd.singularValues()[0] != 0.0) { Sig(0, 0) = 1.0 / svd.singularValues()[0]; } + if (svd.singularValues()[1] != 0.0) { Sig(1, 1) = 1.0 / svd.singularValues()[1]; } + if (svd.singularValues()[2] != 0.0) { Sig(2, 2) = 1.0 / svd.singularValues()[2]; } + if (svd.singularValues()[3] != 0.0) { Sig(3, 3) = 1.0 / svd.singularValues()[3]; } + if (svd.singularValues()[4] != 0.0) { Sig(4, 4) = 1.0 / svd.singularValues()[4]; } + if (svd.singularValues()[5] != 0.0) { Sig(5, 5) = 1.0 / svd.singularValues()[5]; } + + // Pseudoinverse of the mixing matrix + Eigen::Matrix mixer_matrix_pinv = + svd.matrixV() * Sig * svd.matrixU().transpose(); + + // Fill in the mixing matrix from the inverted matrix above + mixer_t inverted_mixer; + + for (int i = 0; i < NUM_MIXER_OUTPUTS; i++) { + inverted_mixer.output_type[i] = mixer_to_invert.output_type[i]; + inverted_mixer.default_pwm_rate[i] = mixer_to_invert.default_pwm_rate[i]; + inverted_mixer.Fx[i] = mixer_matrix_pinv(i, 0); + inverted_mixer.Fy[i] = mixer_matrix_pinv(i, 1); + inverted_mixer.Fz[i] = mixer_matrix_pinv(i, 2); + inverted_mixer.Qx[i] = mixer_matrix_pinv(i, 3); + inverted_mixer.Qy[i] = mixer_matrix_pinv(i, 4); + inverted_mixer.Qz[i] = mixer_matrix_pinv(i, 5); + } + + return inverted_mixer; } -void Mixer::write_motor(uint8_t index, float value) +void Mixer::load_primary_mixer_values() { - 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; + // Load the mixer header values + primary_mixer_.output_type[0] = (output_type_t) RF_.params_.get_param_int(PARAM_PRIMARY_MIXER_OUTPUT_0); + primary_mixer_.output_type[1] = (output_type_t) RF_.params_.get_param_int(PARAM_PRIMARY_MIXER_OUTPUT_1); + primary_mixer_.output_type[2] = (output_type_t) RF_.params_.get_param_int(PARAM_PRIMARY_MIXER_OUTPUT_2); + primary_mixer_.output_type[3] = (output_type_t) RF_.params_.get_param_int(PARAM_PRIMARY_MIXER_OUTPUT_3); + primary_mixer_.output_type[4] = (output_type_t) RF_.params_.get_param_int(PARAM_PRIMARY_MIXER_OUTPUT_4); + primary_mixer_.output_type[5] = (output_type_t) RF_.params_.get_param_int(PARAM_PRIMARY_MIXER_OUTPUT_5); + primary_mixer_.output_type[6] = (output_type_t) RF_.params_.get_param_int(PARAM_PRIMARY_MIXER_OUTPUT_6); + primary_mixer_.output_type[7] = (output_type_t) RF_.params_.get_param_int(PARAM_PRIMARY_MIXER_OUTPUT_7); + primary_mixer_.output_type[8] = (output_type_t) RF_.params_.get_param_int(PARAM_PRIMARY_MIXER_OUTPUT_8); + primary_mixer_.output_type[9] = (output_type_t) RF_.params_.get_param_int(PARAM_PRIMARY_MIXER_OUTPUT_9); + + primary_mixer_.default_pwm_rate[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_0); + primary_mixer_.default_pwm_rate[1] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_1); + primary_mixer_.default_pwm_rate[2] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_2); + primary_mixer_.default_pwm_rate[3] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_3); + primary_mixer_.default_pwm_rate[4] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_4); + primary_mixer_.default_pwm_rate[5] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_5); + primary_mixer_.default_pwm_rate[6] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_6); + primary_mixer_.default_pwm_rate[7] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_7); + primary_mixer_.default_pwm_rate[8] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_8); + primary_mixer_.default_pwm_rate[9] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_9); + + // Load the mixer values from the firmware parameters + for (int i=0; i 1.0) { - value = 1.0; - } else if (value < -1.0) { - value = -1.0; + if (primary_mixer_is_selected_) { + RF_.board_.pwm_init_multi(primary_mixer_.default_pwm_rate, NUM_MIXER_OUTPUTS); + } else { + RF_.board_.pwm_init_multi(esc_calibration_mixing.default_pwm_rate, NUM_MIXER_OUTPUTS); } - 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) @@ -132,43 +439,193 @@ void Mixer::set_new_aux_command(aux_command_t new_aux_command) } } -void Mixer::mix_output() +float Mixer::mix_multirotor_without_motor_parameters(Controller::Output commands) { - Controller::Output commands = RF_.controller_.output(); - float max_output = 1.0f; + // Mix the inputs + float max_output = 1.0; - // 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; + for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) { + if ((*mixer_to_use_.output_type)[i] != AUX) { + // Matrix multiply to mix outputs + outputs_[i] = commands.Fx * (*mixer_to_use_.Fx)[i] + + commands.Fy * (*mixer_to_use_.Fy)[i] + + commands.Fz * (*mixer_to_use_.Fz)[i] + + commands.Qx * (*mixer_to_use_.Qx)[i] + + commands.Qy * (*mixer_to_use_.Qy)[i] + + commands.Qz * (*mixer_to_use_.Qz)[i]; + + // Save off the largest control output (for motors) if it is greater than 1.0 for future scaling + if ((*mixer_to_use_.output_type)[i] == M) { + if (abs(outputs_[i]) > max_output) { max_output = abs(outputs_[i]); } + } + } } - if (mixer_to_use_ == nullptr) { return; } + return max_output; +} + +float Mixer::mix_multirotor_with_motor_parameters(Controller::Output commands) +{ + // Mix the inputs + float max_output = 1.0; + + float rho = RF_.sensors_.rho(); 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]); + if ((*mixer_to_use_.output_type)[i] == M) { + // Matrix multiply to mix outputs for Motor type + float omega_squared = commands.Fx * (*mixer_to_use_.Fx)[i] + + commands.Fy * (*mixer_to_use_.Fy)[i] + + commands.Fz * (*mixer_to_use_.Fz)[i] + + commands.Qx * (*mixer_to_use_.Qx)[i] + + commands.Qy * (*mixer_to_use_.Qy)[i] + + commands.Qz * (*mixer_to_use_.Qz)[i]; + + // Ensure that omega_squared is non-negative + if (omega_squared < 0.0) { omega_squared = 0.0; } + + // Ch. 4, setting equation for torque produced by a propeller equal to Eq. 4.19 + // Note that we assume constant advance ratio, leading to constant torque and thrust constants. + float V_in = rho * pow(D_, 5.0) / (4.0 * pow(M_PI, 2.0)) * omega_squared * C_Q_ * R_ / K_Q_ + + R_ * i_0_ + K_V_ * sqrt(omega_squared); + + // Convert desired V_in setting to a throttle setting + outputs_[i] = V_in / V_max_; + + // Save off the largest control output (for motors) if it is greater than 1.0 for future scaling + if (abs(outputs_[i]) > max_output) { max_output = abs(outputs_[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]; } + } else if ((*mixer_to_use_.output_type)[i] == S) { + // Matrix multiply to mix outputs for Servo type + outputs_[i] = commands.Fx * (*mixer_to_use_.Fx)[i] + + commands.Fy * (*mixer_to_use_.Fy)[i] + + commands.Fz * (*mixer_to_use_.Fz)[i] + + commands.Qx * (*mixer_to_use_.Qx)[i] + + commands.Qy * (*mixer_to_use_.Qy)[i] + + commands.Qz * (*mixer_to_use_.Qz)[i]; } } - // saturate outputs to maintain controllability even during aggressive maneuvers + return max_output; +} + +void Mixer::mix_multirotor() +{ + Controller::Output commands = RF_.controller_.output(); + + // Check the throttle command based on the axis corresponding to the RC F channel + float throttle_command = 0.0; + switch (static_cast(RF_.params_.get_param_int(PARAM_RC_F_AXIS))) { + case X_AXIS: + throttle_command = commands.Fx; + break; + case Y_AXIS: + throttle_command = commands.Fy; + break; + case Z_AXIS: + throttle_command = commands.Fz; + break; + default: + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, + "Invalid RC F axis. Defaulting to z-axis."); + throttle_command = commands.Fz; + break; + } + + if (abs(throttle_command) < 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.Qz = 0.0; + } + + // Mix the outputs based on if a custom mixer (i.e. with motor parameters) is selected. + float max_output; + if (RF_.params_.get_param_int(PARAM_USE_MOTOR_PARAMETERS)) { + max_output = mix_multirotor_with_motor_parameters(commands); + } else { + max_output = mix_multirotor_without_motor_parameters(commands); + } + + // Check to see if the max_output is large. If it is, something is likely wrong with the mixer configuration, so + // warn the user. Note that 2 is an arbitrary value, but seems like a good upper limit since the max output should usually + // be between 0 and 1. + if (max_output > 2.0) { + RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, "Output from mixer is %f! Check mixer", max_output); + } + + // There is no relative scaling on the above equations. In other words, if the input F command is too + // high, then it will "drown out" all other desired outputs. Therefore, we saturate motor 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) - if (mixer_to_use_->output_type[i] == M) { outputs_[i] *= scale_factor; } + if ((*mixer_to_use_.output_type)[i] == M) { outputs_[i] *= scale_factor; } + } +} + +void Mixer::mix_fixedwing() +{ + Controller::Output commands = RF_.controller_.output(); + + // Reverse fixed-wing channels just before mixing if we need to + commands.Qx *= RF_.params_.get_param_int(PARAM_AILERON_REVERSE) ? -1 : 1; + commands.Qy *= RF_.params_.get_param_int(PARAM_ELEVATOR_REVERSE) ? -1 : 1; + commands.Qz *= RF_.params_.get_param_int(PARAM_RUDDER_REVERSE) ? -1 : 1; + + // Mix the outputs + for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) { + if ((*mixer_to_use_.output_type)[i] != AUX) { + // Matrix multiply to mix outputs + outputs_[i] = commands.Fx * (*mixer_to_use_.Fx)[i] + + commands.Fy * (*mixer_to_use_.Fy)[i] + + commands.Fz * (*mixer_to_use_.Fz)[i] + + commands.Qx * (*mixer_to_use_.Qx)[i] + + commands.Qy * (*mixer_to_use_.Qy)[i] + + commands.Qz * (*mixer_to_use_.Qz)[i]; + } + } +} + +void Mixer::select_primary_or_secondary_mixer() +{ + // Check if under RC control. If RC throttle or attitude override is active, + // adjust the mixer_to_use_ accordingly. + if (RF_.command_manager_.rc_throttle_override_active()) { + mixer_to_use_.Fx = &primary_mixer_.Fx; + mixer_to_use_.Fy = &primary_mixer_.Fy; + mixer_to_use_.Fz = &primary_mixer_.Fz; + } else { + mixer_to_use_.Fx = &secondary_mixer_.Fx; + mixer_to_use_.Fy = &secondary_mixer_.Fy; + mixer_to_use_.Fz = &secondary_mixer_.Fz; + } + + if (RF_.command_manager_.rc_attitude_override_active()) { + mixer_to_use_.Qx = &primary_mixer_.Qx; + mixer_to_use_.Qy = &primary_mixer_.Qy; + mixer_to_use_.Qz = &primary_mixer_.Qz; + } else { + mixer_to_use_.Qx = &secondary_mixer_.Qx; + mixer_to_use_.Qy = &secondary_mixer_.Qy; + mixer_to_use_.Qz = &secondary_mixer_.Qz; + } +} + +void Mixer::mix_output() +{ + if (!primary_mixer_is_selected_) { return; } + + // Select the primary or secondary mixer based on the RC override status + select_primary_or_secondary_mixer(); + + // Mix according to airframe type + if (RF_.params_.get_param_int(PARAM_FIXED_WING)) { + mix_fixedwing(); + } else { + mix_multirotor(); } // Insert AUX Commands, and assemble combined_output_types array (Does not override mixer values) @@ -176,11 +633,11 @@ void Mixer::mix_output() // 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) { + if ((*mixer_to_use_.output_type)[i] == AUX) { 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]; + combined_output_type_[i] = (*mixer_to_use_.output_type)[i]; } } diff --git a/src/param.cpp b/src/param.cpp index 6bd0eeb3..bb80f718 100644 --- a/src/param.cpp +++ b/src/param.cpp @@ -108,6 +108,178 @@ void Params::set_defaults(void) 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 + init_param_int(PARAM_NUM_MOTORS, "NUM_MOTORS", 4); // Number of vertical-facing motors on the vehicle | 1 | 8 + init_param_float(PARAM_MOTOR_RESISTANCE, "MOTOR_RESISTANCE", 0.042f); // Electrical resistance of the motor windings (ohms) | 0 | 1000.0 + init_param_float(PARAM_MOTOR_KV, "MOTOR_KV", 0.01706f); // Back emf constant of the motor in SI units (V/rad/s) | 0 | 1000.0 + init_param_float(PARAM_NO_LOAD_CURRENT, "NO_LOAD_CURRENT", 1.5); // No-load current of the motor in amps | 0 | 1000.0 + init_param_float(PARAM_PROP_DIAMETER, "PROP_DIAMETER", 0.381f); // Diameter of the propeller in meters | 0 | 1.0 + init_param_float(PARAM_PROP_CT, "PROP_CT", 0.075f); // Thrust coefficient of the propeller | 0 | 100.0 + init_param_float(PARAM_PROP_CQ, "PROP_CQ", 0.0045f); // Torque coefficient of the propeller | 0 | 100.0 + init_param_float(PARAM_VOLT_MAX, "VOLT_MAX", 25.0f); // Maximum voltage of the battery (V) | 0 | 100.0 + init_param_int(PARAM_USE_MOTOR_PARAMETERS, "USE_MOTOR_PARAM", false); // Flag to use motor parameters in the mixer | 0 | 1 + + init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_0, "PRI_MIXER_OUT_0", 0); // Output type of mixer output 0. | 0 | 1 | 2 | 3 + init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_1, "PRI_MIXER_OUT_1", 0); // Output type of mixer output 1. | 0 | 1 | 2 | 3 + init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_2, "PRI_MIXER_OUT_2", 0); // Output type of mixer output 2. | 0 | 1 | 2 | 3 + init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_3, "PRI_MIXER_OUT_3", 0); // Output type of mixer output 3. | 0 | 1 | 2 | 3 + init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_4, "PRI_MIXER_OUT_4", 0); // Output type of mixer output 4. | 0 | 1 | 2 | 3 + init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_5, "PRI_MIXER_OUT_5", 0); // Output type of mixer output 5. | 0 | 1 | 2 | 3 + init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_6, "PRI_MIXER_OUT_6", 0); // Output type of mixer output 6. | 0 | 1 | 2 | 3 + init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_7, "PRI_MIXER_OUT_7", 0); // Output type of mixer output 7. | 0 | 1 | 2 | 3 + init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_8, "PRI_MIXER_OUT_8", 0); // Output type of mixer output 8. | 0 | 1 | 2 | 3 + init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_9, "PRI_MIXER_OUT_9", 0); // Output type of mixer output 9. | 0 | 1 | 2 | 3 + + init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_0, "PRI_MIXER_PWM_0", 0); // PWM frequenct output for mixer output 0 | 0 | 490 + init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_1, "PRI_MIXER_PWM_1", 0); // PWM frequenct output for mixer output 1 | 0 | 490 + init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_2, "PRI_MIXER_PWM_2", 0); // PWM frequenct output for mixer output 2 | 0 | 490 + init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_3, "PRI_MIXER_PWM_3", 0); // PWM frequenct output for mixer output 3 | 0 | 490 + init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_4, "PRI_MIXER_PWM_4", 0); // PWM frequenct output for mixer output 4 | 0 | 490 + init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_5, "PRI_MIXER_PWM_5", 0); // PWM frequenct output for mixer output 5 | 0 | 490 + init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_6, "PRI_MIXER_PWM_6", 0); // PWM frequenct output for mixer output 6 | 0 | 490 + init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_7, "PRI_MIXER_PWM_7", 0); // PWM frequenct output for mixer output 7 | 0 | 490 + init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_8, "PRI_MIXER_PWM_8", 0); // PWM frequenct output for mixer output 8 | 0 | 490 + init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_9, "PRI_MIXER_PWM_9", 0); // PWM frequenct output for mixer output 9 | 0 | 490 + + init_param_float(PARAM_PRIMARY_MIXER_0_0, "PRI_MIXER_0_0", 0.0f); // Value of the custom mixer at element [0,0] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_1_0, "PRI_MIXER_1_0", 0.0f); // Value of the custom mixer at element [1,0] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_2_0, "PRI_MIXER_2_0", 0.0f); // Value of the custom mixer at element [2,0] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_3_0, "PRI_MIXER_3_0", 0.0f); // Value of the custom mixer at element [3,0] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_4_0, "PRI_MIXER_4_0", 0.0f); // Value of the custom mixer at element [4,0] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_5_0, "PRI_MIXER_5_0", 0.0f); // Value of the custom mixer at element [5,0] | -inf | inf + + init_param_float(PARAM_PRIMARY_MIXER_0_1, "PRI_MIXER_0_1", 0.0f); // Value of the custom mixer at element [0,1] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_1_1, "PRI_MIXER_1_1", 0.0f); // Value of the custom mixer at element [1,1] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_2_1, "PRI_MIXER_2_1", 0.0f); // Value of the custom mixer at element [2,1] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_3_1, "PRI_MIXER_3_1", 0.0f); // Value of the custom mixer at element [3,1] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_4_1, "PRI_MIXER_4_1", 0.0f); // Value of the custom mixer at element [4,1] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_5_1, "PRI_MIXER_5_1", 0.0f); // Value of the custom mixer at element [5,1] | -inf | inf + + init_param_float(PARAM_PRIMARY_MIXER_0_2, "PRI_MIXER_0_2", 0.0f); // Value of the custom mixer at element [0,2] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_1_2, "PRI_MIXER_1_2", 0.0f); // Value of the custom mixer at element [1,2] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_2_2, "PRI_MIXER_2_2", 0.0f); // Value of the custom mixer at element [2,2] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_3_2, "PRI_MIXER_3_2", 0.0f); // Value of the custom mixer at element [3,2] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_4_2, "PRI_MIXER_4_2", 0.0f); // Value of the custom mixer at element [4,2] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_5_2, "PRI_MIXER_5_2", 0.0f); // Value of the custom mixer at element [5,2] | -inf | inf + + init_param_float(PARAM_PRIMARY_MIXER_0_3, "PRI_MIXER_0_3", 0.0f); // Value of the custom mixer at element [0,3] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_1_3, "PRI_MIXER_1_3", 0.0f); // Value of the custom mixer at element [1,3] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_2_3, "PRI_MIXER_2_3", 0.0f); // Value of the custom mixer at element [2,3] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_3_3, "PRI_MIXER_3_3", 0.0f); // Value of the custom mixer at element [3,3] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_4_3, "PRI_MIXER_4_3", 0.0f); // Value of the custom mixer at element [4,3] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_5_3, "PRI_MIXER_5_3", 0.0f); // Value of the custom mixer at element [5,3] | -inf | inf + + init_param_float(PARAM_PRIMARY_MIXER_0_4, "PRI_MIXER_0_4", 0.0f); // Value of the custom mixer at element [0,4] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_1_4, "PRI_MIXER_1_4", 0.0f); // Value of the custom mixer at element [1,4] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_2_4, "PRI_MIXER_2_4", 0.0f); // Value of the custom mixer at element [2,4] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_3_4, "PRI_MIXER_3_4", 0.0f); // Value of the custom mixer at element [3,4] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_4_4, "PRI_MIXER_4_4", 0.0f); // Value of the custom mixer at element [4,4] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_5_4, "PRI_MIXER_5_4", 0.0f); // Value of the custom mixer at element [5,4] | -inf | inf + + init_param_float(PARAM_PRIMARY_MIXER_0_5, "PRI_MIXER_0_5", 0.0f); // Value of the custom mixer at element [0,5] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_1_5, "PRI_MIXER_1_5", 0.0f); // Value of the custom mixer at element [1,5] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_2_5, "PRI_MIXER_2_5", 0.0f); // Value of the custom mixer at element [2,5] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_3_5, "PRI_MIXER_3_5", 0.0f); // Value of the custom mixer at element [3,5] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_4_5, "PRI_MIXER_4_5", 0.0f); // Value of the custom mixer at element [4,5] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_5_5, "PRI_MIXER_5_5", 0.0f); // Value of the custom mixer at element [5,5] | -inf | inf + + init_param_float(PARAM_PRIMARY_MIXER_0_6, "PRI_MIXER_0_6", 0.0f); // Value of the custom mixer at element [0,6] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_1_6, "PRI_MIXER_1_6", 0.0f); // Value of the custom mixer at element [1,6] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_2_6, "PRI_MIXER_2_6", 0.0f); // Value of the custom mixer at element [2,6] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_3_6, "PRI_MIXER_3_6", 0.0f); // Value of the custom mixer at element [3,6] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_4_6, "PRI_MIXER_4_6", 0.0f); // Value of the custom mixer at element [4,6] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_5_6, "PRI_MIXER_5_6", 0.0f); // Value of the custom mixer at element [5,6] | -inf | inf + + init_param_float(PARAM_PRIMARY_MIXER_0_7, "PRI_MIXER_0_7", 0.0f); // Value of the custom mixer at element [0,7] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_1_7, "PRI_MIXER_1_7", 0.0f); // Value of the custom mixer at element [1,7] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_2_7, "PRI_MIXER_2_7", 0.0f); // Value of the custom mixer at element [2,7] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_3_7, "PRI_MIXER_3_7", 0.0f); // Value of the custom mixer at element [3,7] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_4_7, "PRI_MIXER_4_7", 0.0f); // Value of the custom mixer at element [4,7] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_5_7, "PRI_MIXER_5_7", 0.0f); // Value of the custom mixer at element [5,7] | -inf | inf + + init_param_float(PARAM_PRIMARY_MIXER_0_8, "PRI_MIXER_0_8", 0.0f); // Value of the custom mixer at element [0,8] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_1_8, "PRI_MIXER_1_8", 0.0f); // Value of the custom mixer at element [1,8] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_2_8, "PRI_MIXER_2_8", 0.0f); // Value of the custom mixer at element [2,8] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_3_8, "PRI_MIXER_3_8", 0.0f); // Value of the custom mixer at element [3,8] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_4_8, "PRI_MIXER_4_8", 0.0f); // Value of the custom mixer at element [4,8] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_5_8, "PRI_MIXER_5_8", 0.0f); // Value of the custom mixer at element [5,8] | -inf | inf + + init_param_float(PARAM_PRIMARY_MIXER_0_9, "PRI_MIXER_0_9", 0.0f); // Value of the custom mixer at element [0,9] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_1_9, "PRI_MIXER_1_9", 0.0f); // Value of the custom mixer at element [1,9] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_2_9, "PRI_MIXER_2_9", 0.0f); // Value of the custom mixer at element [2,9] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_3_9, "PRI_MIXER_3_9", 0.0f); // Value of the custom mixer at element [3,9] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_4_9, "PRI_MIXER_4_9", 0.0f); // Value of the custom mixer at element [4,9] | -inf | inf + init_param_float(PARAM_PRIMARY_MIXER_5_9, "PRI_MIXER_5_9", 0.0f); // Value of the custom mixer at element [5,9] | -inf | inf + + init_param_float(PARAM_SECONDARY_MIXER_0_0, "SEC_MIXER_0_0", 0.0f); // Value of the custom mixer at element [0,0] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_1_0, "SEC_MIXER_1_0", 0.0f); // Value of the custom mixer at element [1,0] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_2_0, "SEC_MIXER_2_0", 0.0f); // Value of the custom mixer at element [2,0] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_3_0, "SEC_MIXER_3_0", 0.0f); // Value of the custom mixer at element [3,0] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_4_0, "SEC_MIXER_4_0", 0.0f); // Value of the custom mixer at element [4,0] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_5_0, "SEC_MIXER_5_0", 0.0f); // Value of the custom mixer at element [5,0] | -inf | inf + + init_param_float(PARAM_SECONDARY_MIXER_0_1, "SEC_MIXER_0_1", 0.0f); // Value of the custom mixer at element [0,1] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_1_1, "SEC_MIXER_1_1", 0.0f); // Value of the custom mixer at element [1,1] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_2_1, "SEC_MIXER_2_1", 0.0f); // Value of the custom mixer at element [2,1] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_3_1, "SEC_MIXER_3_1", 0.0f); // Value of the custom mixer at element [3,1] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_4_1, "SEC_MIXER_4_1", 0.0f); // Value of the custom mixer at element [4,1] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_5_1, "SEC_MIXER_5_1", 0.0f); // Value of the custom mixer at element [5,1] | -inf | inf + + init_param_float(PARAM_SECONDARY_MIXER_0_2, "SEC_MIXER_0_2", 0.0f); // Value of the custom mixer at element [0,2] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_1_2, "SEC_MIXER_1_2", 0.0f); // Value of the custom mixer at element [1,2] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_2_2, "SEC_MIXER_2_2", 0.0f); // Value of the custom mixer at element [2,2] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_3_2, "SEC_MIXER_3_2", 0.0f); // Value of the custom mixer at element [3,2] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_4_2, "SEC_MIXER_4_2", 0.0f); // Value of the custom mixer at element [4,2] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_5_2, "SEC_MIXER_5_2", 0.0f); // Value of the custom mixer at element [5,2] | -inf | inf + + init_param_float(PARAM_SECONDARY_MIXER_0_3, "SEC_MIXER_0_3", 0.0f); // Value of the custom mixer at element [0,3] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_1_3, "SEC_MIXER_1_3", 0.0f); // Value of the custom mixer at element [1,3] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_2_3, "SEC_MIXER_2_3", 0.0f); // Value of the custom mixer at element [2,3] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_3_3, "SEC_MIXER_3_3", 0.0f); // Value of the custom mixer at element [3,3] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_4_3, "SEC_MIXER_4_3", 0.0f); // Value of the custom mixer at element [4,3] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_5_3, "SEC_MIXER_5_3", 0.0f); // Value of the custom mixer at element [5,3] | -inf | inf + + init_param_float(PARAM_SECONDARY_MIXER_0_4, "SEC_MIXER_0_4", 0.0f); // Value of the custom mixer at element [0,4] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_1_4, "SEC_MIXER_1_4", 0.0f); // Value of the custom mixer at element [1,4] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_2_4, "SEC_MIXER_2_4", 0.0f); // Value of the custom mixer at element [2,4] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_3_4, "SEC_MIXER_3_4", 0.0f); // Value of the custom mixer at element [3,4] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_4_4, "SEC_MIXER_4_4", 0.0f); // Value of the custom mixer at element [4,4] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_5_4, "SEC_MIXER_5_4", 0.0f); // Value of the custom mixer at element [5,4] | -inf | inf + + init_param_float(PARAM_SECONDARY_MIXER_0_5, "SEC_MIXER_0_5", 0.0f); // Value of the custom mixer at element [0,5] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_1_5, "SEC_MIXER_1_5", 0.0f); // Value of the custom mixer at element [1,5] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_2_5, "SEC_MIXER_2_5", 0.0f); // Value of the custom mixer at element [2,5] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_3_5, "SEC_MIXER_3_5", 0.0f); // Value of the custom mixer at element [3,5] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_4_5, "SEC_MIXER_4_5", 0.0f); // Value of the custom mixer at element [4,5] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_5_5, "SEC_MIXER_5_5", 0.0f); // Value of the custom mixer at element [5,5] | -inf | inf + + init_param_float(PARAM_SECONDARY_MIXER_0_6, "SEC_MIXER_0_6", 0.0f); // Value of the custom mixer at element [0,6] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_1_6, "SEC_MIXER_1_6", 0.0f); // Value of the custom mixer at element [1,6] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_2_6, "SEC_MIXER_2_6", 0.0f); // Value of the custom mixer at element [2,6] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_3_6, "SEC_MIXER_3_6", 0.0f); // Value of the custom mixer at element [3,6] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_4_6, "SEC_MIXER_4_6", 0.0f); // Value of the custom mixer at element [4,6] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_5_6, "SEC_MIXER_5_6", 0.0f); // Value of the custom mixer at element [5,6] | -inf | inf + + init_param_float(PARAM_SECONDARY_MIXER_0_7, "SEC_MIXER_0_7", 0.0f); // Value of the custom mixer at element [0,7] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_1_7, "SEC_MIXER_1_7", 0.0f); // Value of the custom mixer at element [1,7] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_2_7, "SEC_MIXER_2_7", 0.0f); // Value of the custom mixer at element [2,7] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_3_7, "SEC_MIXER_3_7", 0.0f); // Value of the custom mixer at element [3,7] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_4_7, "SEC_MIXER_4_7", 0.0f); // Value of the custom mixer at element [4,7] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_5_7, "SEC_MIXER_5_7", 0.0f); // Value of the custom mixer at element [5,7] | -inf | inf + + init_param_float(PARAM_SECONDARY_MIXER_0_8, "SEC_MIXER_0_8", 0.0f); // Value of the custom mixer at element [0,8] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_1_8, "SEC_MIXER_1_8", 0.0f); // Value of the custom mixer at element [1,8] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_2_8, "SEC_MIXER_2_8", 0.0f); // Value of the custom mixer at element [2,8] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_3_8, "SEC_MIXER_3_8", 0.0f); // Value of the custom mixer at element [3,8] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_4_8, "SEC_MIXER_4_8", 0.0f); // Value of the custom mixer at element [4,8] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_5_8, "SEC_MIXER_5_8", 0.0f); // Value of the custom mixer at element [5,8] | -inf | inf + + init_param_float(PARAM_SECONDARY_MIXER_0_9, "SEC_MIXER_0_9", 0.0f); // Value of the custom mixer at element [0,9] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_1_9, "SEC_MIXER_1_9", 0.0f); // Value of the custom mixer at element [1,9] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_2_9, "SEC_MIXER_2_9", 0.0f); // Value of the custom mixer at element [2,9] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_3_9, "SEC_MIXER_3_9", 0.0f); // Value of the custom mixer at element [3,9] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_4_9, "SEC_MIXER_4_9", 0.0f); // Value of the custom mixer at element [4,9] | -inf | inf + init_param_float(PARAM_SECONDARY_MIXER_5_9, "SEC_MIXER_5_9", 0.0f); // Value of the custom mixer at element [5,9] | -inf | inf + /*****************************/ /*** MAVLINK CONFIGURATION ***/ /*****************************/ @@ -116,8 +288,6 @@ void Params::set_defaults(void) /********************************/ /*** 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 @@ -208,6 +378,7 @@ void Params::set_defaults(void) 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_F_AXIS, "RC_F_AXIS", 2); // NED axis that RC F-channel gets mapped to 0 - X, 1 - Y, 2 - Z | 0 | 2 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 @@ -222,6 +393,7 @@ void Params::set_defaults(void) 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_float(PARAM_RC_MAX_THROTTLE, "RC_MAX_THR", 0.7f); // Maximum throttle command sent by full deflection of RC sticks, to maintain controllability during aggressive maneuvers | 0.0 | 1.0 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 @@ -233,7 +405,8 @@ void Params::set_defaults(void) /***************************/ /*** FRAME CONFIGURATION ***/ /***************************/ - init_param_int(PARAM_MIXER, "MIXER", Mixer::INVALID_MIXER); // Which mixer to choose - See Mixer documentation | 0 | 10 + init_param_int(PARAM_PRIMARY_MIXER, "PRIMARY_MIXER", Mixer::INVALID_MIXER); // Which mixer to choose for primary mixer - See Mixer documentation | 0 | 11 + init_param_int(PARAM_SECONDARY_MIXER, "SECONDARY_MIXER", Mixer::INVALID_MIXER); // Which mixer to choose for secondary mixer - See Mixer documentation | 0 | 11 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 @@ -253,10 +426,10 @@ void Params::set_defaults(void) /*****************************/ /*** 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); + init_param_float(PARAM_BATTERY_VOLTAGE_MULTIPLIER, "BATT_VOLT_MULT", 0.0f); // Multiplier for the voltage sensor | 0 | inf + init_param_float(PARAM_BATTERY_CURRENT_MULTIPLIER, "BATT_CURR_MULT", 0.0f); // Multiplier for the current sensor | 0 | inf + init_param_float(PARAM_BATTERY_VOLTAGE_ALPHA, "BATT_VOLT_ALPHA", 0.995f); // Alpha value for the low pass filter on the reported battery voltage | 0 | 1 + init_param_float(PARAM_BATTERY_CURRENT_ALPHA, "BATT_CURR_ALPHA", 0.995f); // Alpha value for the low pass filter on the reported battery current | 0 | 1 /************************/ /*** OFFBOARD CONTROL ***/ diff --git a/src/sensors.cpp b/src/sensors.cpp index fb491836..6dad39dd 100644 --- a/src/sensors.cpp +++ b/src/sensors.cpp @@ -174,6 +174,8 @@ got_flags Sensors::run() got.baro = true; rf_.board_.baro_read(&data_.baro_pressure, &data_.baro_temperature); correct_baro(); + + rho_ = (0.0289644 * data_.baro_pressure) / (8.31432 * data_.baro_temperature); } } diff --git a/test/command_manager_test.cpp b/test/command_manager_test.cpp index 5226f67b..7ce644e7 100644 --- a/test/command_manager_test.cpp +++ b/test/command_manager_test.cpp @@ -9,10 +9,12 @@ #define CHN_LOW 1100 #define CHN_HIGH 1900 -#define OFFBOARD_X -1.0 -#define OFFBOARD_Y 0.5 -#define OFFBOARD_Z -0.7 -#define OFFBOARD_F 0.9 +#define OFFBOARD_QX -1.0 +#define OFFBOARD_QY 0.5 +#define OFFBOARD_QZ -0.7 +#define OFFBOARD_FX 0.9 +#define OFFBOARD_FY 0.9 +#define OFFBOARD_FZ 0.9 #define RC_X_PWM 1800 #define RC_X ((RC_X_PWM - 1500) / 500.0 * rf.params_.get_param_float(PARAM_RC_MAX_ROLL)) @@ -31,10 +33,12 @@ class CommandManagerTest : public ::testing::Test float max_roll, max_pitch, max_yawrate; control_t offboard_command = {20000, - {true, ANGLE, OFFBOARD_X}, - {true, ANGLE, OFFBOARD_Y}, - {true, RATE, OFFBOARD_Z}, - {true, THROTTLE, OFFBOARD_F}}; + {true, ANGLE, OFFBOARD_QX}, + {true, ANGLE, OFFBOARD_QY}, + {true, RATE, OFFBOARD_QZ}, + {true, THROTTLE, OFFBOARD_FX} + {true, THROTTLE, OFFBOARD_FY} + {true, THROTTLE, OFFBOARD_FZ}}; CommandManagerTest() : mavlink(board) @@ -51,6 +55,7 @@ class CommandManagerTest : public ::testing::Test for (int i = 0; i < 8; i++) { rc_values[i] = 1500; } rc_values[2] = 1000; + // TODO: Fix this unit test. There is no passthrough mixer rf.params_.set_param_int(PARAM_MIXER, Mixer::PASSTHROUGH); rf.params_.set_param_float(PARAM_FAILSAFE_THROTTLE, 0.0); max_roll = rf.params_.get_param_float(PARAM_RC_MAX_ROLL); @@ -85,14 +90,14 @@ TEST_F(CommandManagerTest, Default) stepFirmware(20000); control_t output = rf.command_manager_.combined_control(); - EXPECT_EQ(output.x.type, ANGLE); - EXPECT_CLOSE(output.x.value, 0.0); - EXPECT_EQ(output.y.type, ANGLE); - EXPECT_CLOSE(output.y.value, 0.0); - EXPECT_EQ(output.z.type, RATE); - EXPECT_CLOSE(output.z.value, 0.0); - EXPECT_EQ(output.F.type, THROTTLE); - EXPECT_CLOSE(output.F.value, 0.0); + EXPECT_EQ(output.Qx.type, ANGLE); + EXPECT_CLOSE(output.Qx.value, 0.0); + EXPECT_EQ(output.Qy.type, ANGLE); + EXPECT_CLOSE(output.Qy.value, 0.0); + EXPECT_EQ(output.Qz.type, RATE); + EXPECT_CLOSE(output.Qz.value, 0.0); + EXPECT_EQ(output.Fz.type, THROTTLE); + EXPECT_CLOSE(output.Fz.value, 0.0); } TEST_F(CommandManagerTest, RCCommands) @@ -104,14 +109,14 @@ TEST_F(CommandManagerTest, RCCommands) stepFirmware(50000); control_t output = rf.command_manager_.combined_control(); - EXPECT_EQ(output.x.type, ANGLE); - EXPECT_CLOSE(output.x.value, 1.0 * max_roll); - EXPECT_EQ(output.y.type, ANGLE); - EXPECT_CLOSE(output.y.value, -1.0 * max_pitch); - EXPECT_EQ(output.z.type, RATE); - EXPECT_CLOSE(output.z.value, -0.5 * max_yawrate); - EXPECT_EQ(output.F.type, THROTTLE); - EXPECT_CLOSE(output.F.value, 0.5); + EXPECT_EQ(output.Qx.type, ANGLE); + EXPECT_CLOSE(output.Qx.value, 1.0 * max_roll); + EXPECT_EQ(output.Qy.type, ANGLE); + EXPECT_CLOSE(output.Qy.value, -1.0 * max_pitch); + EXPECT_EQ(output.Qz.type, RATE); + EXPECT_CLOSE(output.Qz.value, -0.5 * max_yawrate); + EXPECT_EQ(output.Fz.type, THROTTLE); + EXPECT_CLOSE(output.Fz.value, 0.5); } TEST_F(CommandManagerTest, ArmWithSticksByDefault) @@ -207,14 +212,14 @@ TEST_F(CommandManagerTest, DefaultRCOutputd) // Check the output control_t output = rf.command_manager_.combined_control(); - EXPECT_EQ(output.x.type, ANGLE); - EXPECT_CLOSE(output.x.value, 0.0); - EXPECT_EQ(output.y.type, ANGLE); - EXPECT_CLOSE(output.y.value, 0.0); - EXPECT_EQ(output.z.type, RATE); - EXPECT_CLOSE(output.z.value, 0.0); - EXPECT_EQ(output.F.type, THROTTLE); - EXPECT_CLOSE(output.F.value, 0.0); + EXPECT_EQ(output.Qx.type, ANGLE); + EXPECT_CLOSE(output.Qx.value, 0.0); + EXPECT_EQ(output.Qy.type, ANGLE); + EXPECT_CLOSE(output.Qy.value, 0.0); + EXPECT_EQ(output.Qz.type, RATE); + EXPECT_CLOSE(output.Qz.value, 0.0); + EXPECT_EQ(output.Fz.type, THROTTLE); + EXPECT_CLOSE(output.Fz.value, 0.0); } TEST_F(CommandManagerTest, RCOutput) @@ -228,14 +233,14 @@ TEST_F(CommandManagerTest, RCOutput) // Check the output EXPECT_EQ(rf.state_manager_.state().armed, false); control_t output = rf.command_manager_.combined_control(); - EXPECT_EQ(output.x.type, ANGLE); - EXPECT_CLOSE(output.x.value, max_roll * -0.5); - EXPECT_EQ(output.y.type, ANGLE); - EXPECT_CLOSE(output.y.value, max_pitch * 0.5); - EXPECT_EQ(output.z.type, RATE); - EXPECT_CLOSE(output.z.value, max_yawrate); - EXPECT_EQ(output.F.type, THROTTLE); - EXPECT_CLOSE(output.F.value, 0.5); + EXPECT_EQ(output.Qx.type, ANGLE); + EXPECT_CLOSE(output.Qx.value, max_roll * -0.5); + EXPECT_EQ(output.Qy.type, ANGLE); + EXPECT_CLOSE(output.Qy.value, max_pitch * 0.5); + EXPECT_EQ(output.Qz.type, RATE); + EXPECT_CLOSE(output.Qz.value, max_yawrate); + EXPECT_EQ(output.Fz.type, THROTTLE); + EXPECT_CLOSE(output.Fz.value, 0.5); } TEST_F(CommandManagerTest, LoseRCDisarmed) @@ -244,14 +249,14 @@ TEST_F(CommandManagerTest, LoseRCDisarmed) stepFirmware(50000); control_t output = rf.command_manager_.combined_control(); - EXPECT_EQ(output.x.type, ANGLE); - EXPECT_CLOSE(output.x.value, 0.0 * max_roll); - EXPECT_EQ(output.y.type, ANGLE); - EXPECT_CLOSE(output.y.value, 0.0 * max_pitch); - EXPECT_EQ(output.z.type, RATE); - EXPECT_CLOSE(output.z.value, 0.0 * max_yawrate); - EXPECT_EQ(output.F.type, THROTTLE); - EXPECT_CLOSE(output.F.value, 0.0); + EXPECT_EQ(output.Qx.type, ANGLE); + EXPECT_CLOSE(output.Qx.value, 0.0 * max_roll); + EXPECT_EQ(output.Qy.type, ANGLE); + EXPECT_CLOSE(output.Qy.value, 0.0 * max_pitch); + EXPECT_EQ(output.Qz.type, RATE); + EXPECT_CLOSE(output.Qz.value, 0.0 * max_yawrate); + EXPECT_EQ(output.Fz.type, THROTTLE); + EXPECT_CLOSE(output.Fz.value, 0.0); // We should also be disarmed and in error EXPECT_EQ(rf.state_manager_.state().armed, false); @@ -281,14 +286,14 @@ TEST_F(CommandManagerTest, LoseRCArmed) stepFirmware(20000); control_t output = rf.command_manager_.combined_control(); - EXPECT_EQ(output.x.type, ANGLE); - EXPECT_CLOSE(output.x.value, 0.0 * max_roll); - EXPECT_EQ(output.y.type, ANGLE); - EXPECT_CLOSE(output.y.value, 0.0 * max_pitch); - EXPECT_EQ(output.z.type, RATE); - EXPECT_CLOSE(output.z.value, 0.0 * max_yawrate); - EXPECT_EQ(output.F.type, THROTTLE); - EXPECT_CLOSE(output.F.value, rf.params_.get_param_float(PARAM_FAILSAFE_THROTTLE)); + EXPECT_EQ(output.Qx.type, ANGLE); + EXPECT_CLOSE(output.Qx.value, 0.0 * max_roll); + EXPECT_EQ(output.Qy.type, ANGLE); + EXPECT_CLOSE(output.Qy.value, 0.0 * max_pitch); + EXPECT_EQ(output.Qz.type, RATE); + EXPECT_CLOSE(output.Qz.value, 0.0 * max_yawrate); + EXPECT_EQ(output.Fz.type, THROTTLE); + EXPECT_CLOSE(output.Fz.value, rf.params_.get_param_float(PARAM_FAILSAFE_THROTTLE)); // We should also be disarmed and in error EXPECT_EQ(rf.state_manager_.state().armed, true); @@ -323,10 +328,12 @@ TEST_F(CommandManagerTest, OffboardCommandMuxNoMinThrottle) stepFirmware(20000); control_t output = rf.command_manager_.combined_control(); - EXPECT_CLOSE(output.x.value, OFFBOARD_X); - EXPECT_CLOSE(output.y.value, OFFBOARD_Y); - EXPECT_CLOSE(output.z.value, OFFBOARD_Z); - EXPECT_CLOSE(output.F.value, OFFBOARD_F); + EXPECT_CLOSE(output.Qx.value, OFFBOARD_QX); + EXPECT_CLOSE(output.Qy.value, OFFBOARD_QY); + EXPECT_CLOSE(output.Qz.value, OFFBOARD_QZ); + EXPECT_CLOSE(output.Fx.value, OFFBOARD_FX); + EXPECT_CLOSE(output.Fy.value, OFFBOARD_FY); + EXPECT_CLOSE(output.Fz.value, OFFBOARD_FZ); } TEST_F(CommandManagerTest, OffboardCommandMuxMinThrottle) @@ -340,10 +347,12 @@ TEST_F(CommandManagerTest, OffboardCommandMuxMinThrottle) stepFirmware(20000); control_t output = rf.command_manager_.combined_control(); - EXPECT_CLOSE(output.x.value, OFFBOARD_X); - EXPECT_CLOSE(output.y.value, OFFBOARD_Y); - EXPECT_CLOSE(output.z.value, OFFBOARD_Z); - EXPECT_CLOSE(output.F.value, 0.0); + EXPECT_CLOSE(output.Qx.value, OFFBOARD_QX); + EXPECT_CLOSE(output.Qy.value, OFFBOARD_QY); + EXPECT_CLOSE(output.Qz.value, OFFBOARD_QZ); + EXPECT_CLOSE(output.Fx.value, OFFBOARD_FX); + EXPECT_CLOSE(output.Fy.value, OFFBOARD_FY); + EXPECT_CLOSE(output.Fz.value, 0.0); } TEST_F(CommandManagerTest, OffboardCommandMuxRollDeviation) @@ -355,10 +364,10 @@ TEST_F(CommandManagerTest, OffboardCommandMuxRollDeviation) stepFirmware(40000); control_t output = rf.command_manager_.combined_control(); - EXPECT_CLOSE(output.x.value, -0.5 * rf.params_.get_param_float(PARAM_RC_MAX_ROLL)); - EXPECT_CLOSE(output.y.value, OFFBOARD_Y); - EXPECT_CLOSE(output.z.value, OFFBOARD_Z); - EXPECT_CLOSE(output.F.value, 0.0); + EXPECT_CLOSE(output.Qx.value, -0.5 * rf.params_.get_param_float(PARAM_RC_MAX_ROLL)); + EXPECT_CLOSE(output.Qy.value, OFFBOARD_QY); + EXPECT_CLOSE(output.Qz.value, OFFBOARD_QZ); + EXPECT_CLOSE(output.Fz.value, 0.0); } TEST_F(CommandManagerTest, OffboardCommandMuxPitchDeviation) @@ -370,10 +379,10 @@ TEST_F(CommandManagerTest, OffboardCommandMuxPitchDeviation) stepFirmware(40000); control_t output = rf.command_manager_.combined_control(); - EXPECT_CLOSE(output.x.value, OFFBOARD_X); - EXPECT_CLOSE(output.y.value, 0.5 * rf.params_.get_param_float(PARAM_RC_MAX_PITCH)); - EXPECT_CLOSE(output.z.value, OFFBOARD_Z); - EXPECT_CLOSE(output.F.value, 0.0); + EXPECT_CLOSE(output.Qx.value, OFFBOARD_QX); + EXPECT_CLOSE(output.Qy.value, 0.5 * rf.params_.get_param_float(PARAM_RC_MAX_PITCH)); + EXPECT_CLOSE(output.Qz.value, OFFBOARD_QZ); + EXPECT_CLOSE(output.Fz.value, 0.0); } TEST_F(CommandManagerTest, OffboardCommandMuxYawrateDeviation) @@ -385,10 +394,10 @@ TEST_F(CommandManagerTest, OffboardCommandMuxYawrateDeviation) stepFirmware(40000); control_t output = rf.command_manager_.combined_control(); - EXPECT_CLOSE(output.x.value, OFFBOARD_X); - EXPECT_CLOSE(output.y.value, OFFBOARD_Y); - EXPECT_CLOSE(output.z.value, -0.5 * rf.params_.get_param_float(PARAM_RC_MAX_YAWRATE)); - EXPECT_CLOSE(output.F.value, 0.0); + EXPECT_CLOSE(output.Qx.value, OFFBOARD_QX); + EXPECT_CLOSE(output.Qy.value, OFFBOARD_QY); + EXPECT_CLOSE(output.Qz.value, -0.5 * rf.params_.get_param_float(PARAM_RC_MAX_YAWRATE)); + EXPECT_CLOSE(output.Fz.value, 0.0); } TEST_F(CommandManagerTest, OffboardCommandMuxLag) @@ -400,24 +409,24 @@ TEST_F(CommandManagerTest, OffboardCommandMuxLag) stepFirmware(40000); control_t output = rf.command_manager_.combined_control(); - EXPECT_CLOSE(output.x.value, -0.5 * rf.params_.get_param_float(PARAM_RC_MAX_ROLL)); + EXPECT_CLOSE(output.Qx.value, -0.5 * rf.params_.get_param_float(PARAM_RC_MAX_ROLL)); rc_values[0] = 1500; // return stick to center stepFirmware(500000); setOffboard(offboard_command); output = rf.command_manager_.combined_control(); - EXPECT_CLOSE(output.x.value, 0.0); // lag + EXPECT_CLOSE(output.Qx.value, 0.0); // lag stepFirmware(600000); setOffboard(offboard_command); output = rf.command_manager_.combined_control(); - EXPECT_CLOSE(output.x.value, 0.0); // lag + EXPECT_CLOSE(output.Qx.value, 0.0); // lag setOffboard(offboard_command); stepFirmware(20000); output = rf.command_manager_.combined_control(); - EXPECT_CLOSE(output.x.value, OFFBOARD_X); + EXPECT_CLOSE(output.Qx.value, OFFBOARD_QX); } TEST_F(CommandManagerTest, StaleOffboardCommand) @@ -430,33 +439,33 @@ TEST_F(CommandManagerTest, StaleOffboardCommand) stepFirmware(timeout_us + 40000); control_t output = rf.command_manager_.combined_control(); - EXPECT_CLOSE(output.x.value, 0.0); + EXPECT_CLOSE(output.Qx.value, 0.0); } TEST_F(CommandManagerTest, PartialMux) { - offboard_command.x.active = false; + offboard_command.Qx.active = false; stepFirmware(1000000); setOffboard(offboard_command); stepFirmware(30000); control_t output = rf.command_manager_.combined_control(); - EXPECT_CLOSE(output.x.value, 0.0); - EXPECT_CLOSE(output.y.value, OFFBOARD_Y); - EXPECT_CLOSE(output.z.value, OFFBOARD_Z); - EXPECT_CLOSE(output.F.value, 0.0); + EXPECT_CLOSE(output.Qx.value, 0.0); + EXPECT_CLOSE(output.Qy.value, OFFBOARD_QY); + EXPECT_CLOSE(output.Qz.value, OFFBOARD_QZ); + EXPECT_CLOSE(output.Fz.value, 0.0); } TEST_F(CommandManagerTest, MixedTypes) { - offboard_command.x.type = RATE; + offboard_command.Qx.type = RATE; stepFirmware(1000000); setOffboard(offboard_command); stepFirmware(30000); control_t output = rf.command_manager_.combined_control(); - EXPECT_EQ(output.x.type, RATE); - EXPECT_EQ(output.y.type, ANGLE); - EXPECT_EQ(output.z.type, RATE); - EXPECT_EQ(output.F.type, THROTTLE); + EXPECT_EQ(output.Qx.type, RATE); + EXPECT_EQ(output.Qy.type, ANGLE); + EXPECT_EQ(output.Qz.type, RATE); + EXPECT_EQ(output.Fz.type, THROTTLE); } diff --git a/test/parameters_test.cpp b/test/parameters_test.cpp index b54be57b..a0f4a4b2 100644 --- a/test/parameters_test.cpp +++ b/test/parameters_test.cpp @@ -66,7 +66,7 @@ TEST(Parameters, DefaultParameters) EXPECT_PARAM_EQ_FLOAT(PARAM_RC_OVERRIDE_DEVIATION, 0.1f); EXPECT_PARAM_EQ_INT(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, 1); EXPECT_PARAM_EQ_INT(PARAM_RC_ATTITUDE_MODE, 1); - EXPECT_PARAM_EQ_INT(PARAM_MIXER, Mixer::INVALID_MIXER); + EXPECT_PARAM_EQ_INT(PARAM_PRIMARY_MIXER, Mixer::INVALID_MIXER); EXPECT_PARAM_EQ_INT(PARAM_FIXED_WING, 0); EXPECT_PARAM_EQ_INT(PARAM_AILERON_REVERSE, 0); EXPECT_PARAM_EQ_INT(PARAM_ELEVATOR_REVERSE, 0); diff --git a/test/state_machine_test.cpp b/test/state_machine_test.cpp index 235f5f87..40272292 100644 --- a/test/state_machine_test.cpp +++ b/test/state_machine_test.cpp @@ -25,7 +25,7 @@ class StateMachineTest : public ::testing::Test rf.init(); rf.state_manager_.clear_error( rf.state_manager_.state().error_codes); // Clear All Errors to Start - rf.params_.set_param_int(PARAM_MIXER, 10); + rf.params_.set_param_int(PARAM_PRIMARY_MIXER, 10); rf.params_.set_param_int(PARAM_CALIBRATE_GYRO_ON_ARM, false); // default to turning this off rf.params_.set_param_float(PARAM_FAILSAFE_THROTTLE, 0.0f); @@ -515,4 +515,4 @@ TEST_F(StateMachineTest, ArmAfterCorrectFailsafe) EXPECT_EQ(rf.state_manager_.state().armed, true); EXPECT_EQ(rf.state_manager_.state().error_codes, 0); EXPECT_EQ(rf.state_manager_.state().error, false); -} \ No newline at end of file +}