diff --git a/boards/varmint_h7 b/boards/varmint_h7 index f7a80658..1165c798 160000 --- a/boards/varmint_h7 +++ b/boards/varmint_h7 @@ -1 +1 @@ -Subproject commit f7a80658ef924d81974ce13329d5da5d8640d7f6 +Subproject commit 1165c7986da8d3bb3adda4286d9036a5858837a7 diff --git a/include/board.h b/include/board.h index 2b350b97..15dbe117 100644 --- a/include/board.h +++ b/include/board.h @@ -116,8 +116,10 @@ class Board // PWM virtual void pwm_init(uint32_t refresh_rate, uint16_t idle_pwm) = 0; + virtual void pwm_init_multi(const float * rate, uint32_t channels) = 0; virtual void pwm_disable() = 0; virtual void pwm_write(uint8_t channel, float value) = 0; + virtual void pwm_write_multi(float * value, uint32_t channels) = 0; // non-volatile memory virtual void memory_init() = 0; diff --git a/include/mixer.h b/include/mixer.h index d11359b6..7a0b7a13 100644 --- a/include/mixer.h +++ b/include/mixer.h @@ -45,7 +45,7 @@ class Mixer : public ParamListenerInterface { public: static constexpr uint8_t NUM_TOTAL_OUTPUTS = 14; - static constexpr uint8_t NUM_MIXER_OUTPUTS = 8; + static constexpr uint8_t NUM_MIXER_OUTPUTS = 10; enum { @@ -62,7 +62,8 @@ class Mixer : public ParamListenerInterface FIXEDWING = 10, PASSTHROUGH = 11, VTAIL = 12, - CUSTOM = 13, + QUADPLANE = 13, + CUSTOM = 14, NUM_MIXERS, INVALID_MIXER = 255 }; @@ -78,11 +79,11 @@ class Mixer : public ParamListenerInterface typedef struct { 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]; - uint32_t default_pwm_rate; } mixer_t; typedef struct @@ -107,143 +108,148 @@ class Mixer : public ParamListenerInterface void write_motor(uint8_t index, float value); void write_servo(uint8_t index, float value); - const mixer_t esc_calibration_mixing = {{M, M, M, M, M, M, NONE, NONE}, + // clang-format off - {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f}, // F Mix - {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - 490}; + const mixer_t esc_calibration_mixing = { + { M, M, M, M, M, M, NONE, NONE, NONE, NONE}, // 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 const mixer_t quadcopter_plus_mixing = { - {M, M, M, M, NONE, NONE, NONE, NONE}, // output_type - - {1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix - {0.0f, -1.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - {1.0f, 0.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix - {1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Z Mix - 490}; - - const mixer_t quadcopter_x_mixing = {{M, M, M, M, NONE, NONE, NONE, NONE}, // output_type - - {1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix - {-1.0f, -1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - {1.0f, -1.0f, -1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix - {1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Z Mix - 490}; + { 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 + + 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 const mixer_t hex_plus_mixing = { - {M, M, M, M, M, M, M, M}, // output_type - - {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // F Mix - {0.0f, -0.866025f, -0.866025f, 0.0f, 0.866025f, 0.866025f, 0.0f, 0.0f}, // X Mix - {1.0f, 0.5f, -0.5f, -1.0f, -0.5f, 0.5f, 0.0f, 0.0f}, // Y Mix - {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f}, // Z Mix - 490}; + { 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 const mixer_t hex_x_mixing = { - {M, M, M, M, M, M, M, M}, // output_type - - {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // F Mix - {-0.5f, -1.0f, -0.5f, 0.5f, 1.0f, 0.5f, 0.0f, 0.0f}, // X Mix - {0.866025f, 0.0f, -0.866025f, -0.866025f, 0.0f, 0.866025f, 0.0f, 0.0f}, // Y Mix - {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f}, // Z Mix - 490}; + { 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 const mixer_t octocopter_plus_mixing = { - {M, M, M, M, M, M, M, M}, // output_type - - {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f}, // F Mix - {0.0f, -0.707f, -1.0f, -0.707f, 0.0f, 0.707f, 1.0f, 0.707f}, // X Mix - {1.0f, 0.707f, 0.0f, -0.707f, -1.0f, -0.707f, 0.0f, 0.707f}, // Y Mix - {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f}, // Z Mix - 490}; + { 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 const mixer_t octocopter_x_mixing = { - {M, M, M, M, M, M, M, M}, // output_type - - {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f}, // F Mix - {-0.414f, -1.0f, -1.0f, -0.414f, 0.414f, 1.0f, 1.0f, 0.414}, // X Mix - {1.0f, 0.414f, -0.414f, -1.0f, -1.0f, -0.414f, 0.414f, 1.0}, // Y Mix - {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f}, // Z Mix - 490}; + { 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 const mixer_t Y6_mixing = { - {M, M, M, M, M, M, NONE, NONE}, // output_type - - {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // F Mix - {-1.0f, -1.0f, 0.0f, 0.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // X Mix - {0.667f, 0.667f, -1.333f, -1.333f, 0.667f, 0.667f, 0.0f, 0.0f}, // Y Mix - {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f}, // Z Mix - 490}; - - const mixer_t X8_mixing = {{M, M, M, M, M, M, M, M}, // output_type - - {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f}, // F Mix - {-1.0f, -1.0f, -1.0f, -1.0f, 1.0f, 1.0f, 1.0f, 1.0f}, // X Mix - {1.0f, 1.0f, -1.0f, -1.0f, -1.0f, -1.0f, 1.0f, 1.0f}, // Y Mix - {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f}, // Z Mix - 490}; + { 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 + + 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, S, NONE, NONE, NONE, NONE}, // output_type - - {1.0f, 0.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix - {-1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - {0.667f, 0.0f, 0.667f, -1.333f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix - {0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Z Mix - 490}; - - const mixer_t fixedwing_mixing = {{S, S, M, S, S, M, NONE, NONE}, // output type - - {0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix - {1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - {0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix - {0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Z Mix - 50}; - - const mixer_t passthrough_mixing = {{NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE}, - - {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix - {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix - {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Z Mix - 50}; + { M, M, M, NONE, S, NONE, NONE, NONE, NONE, NONE}, // output_type + { 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 + + const mixer_t fixedwing_mixing = { + { S, S, S, NONE, M, NONE, NONE, NONE, NONE, NONE}, // 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, S, M, S, NONE, NONE, NONE, NONE}, // Ailerons, LRuddervator, Motor, RRuddervator - - {0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix - {1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - {0.0f, -0.5f, 0.0f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix - {0.0f, 0.5f, 0.0f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f}, // Z Mix - 50}; - - const mixer_t custom_mixing = {{NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE}, // output type - - {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix - {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix - {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Z Mix - 50}; + { S, S, S, NONE, M, NONE, NONE, NONE, NONE, NONE}, // Ailerons, LRuddervator, RRuddervator, Motor + { 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, -0.5f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + {0.0f, 0.5f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix + + const mixer_t quadplane_mixing = { + { S, S, S, M, M, M, M, M, NONE, NONE}, // Ailerons, Rudder, Elevator, Tractor Motor, Quadrotors + { 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 + + const mixer_t custom_mixing = { + {NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE}, // 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_; - // clang-format off - 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, - &custom_mixing}; + 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 + }; + // clang-format on public: diff --git a/src/mixer.cpp b/src/mixer.cpp index 2df2c9cc..1115eb7a 100644 --- a/src/mixer.cpp +++ b/src/mixer.cpp @@ -88,15 +88,11 @@ void Mixer::init_mixing() void Mixer::init_PWM() { - uint32_t refresh_rate = RF_.params_.get_param_int(PARAM_MOTOR_PWM_SEND_RATE); - if (refresh_rate == 0 && mixer_to_use_ != nullptr) { - refresh_rate = mixer_to_use_->default_pwm_rate; + 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); } - int16_t off_pwm = 1000; - - if (mixer_to_use_ == nullptr || refresh_rate == 0) RF_.board_.pwm_init(50, 0); - else - RF_.board_.pwm_init(refresh_rate, off_pwm); } void Mixer::write_motor(uint8_t index, float value) @@ -172,7 +168,7 @@ void Mixer::mix_output() // Perform Motor Output Scaling for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) { // scale all motor outputs by scale factor (this is usually 1.0, unless we saturated) - outputs_[i] *= scale_factor; + if (mixer_to_use_->output_type[i] == M) { outputs_[i] *= scale_factor; } } // Insert AUX Commands, and assemble combined_output_types array (Does not override mixer values) @@ -196,12 +192,31 @@ void Mixer::mix_output() // Write to outputs for (uint8_t i = 0; i < NUM_TOTAL_OUTPUTS; i++) { + float value = outputs_[i]; if (combined_output_type_[i] == S) { - write_servo(i, outputs_[i]); + if (value > 1.0) { + value = 1.0; + } else if (value < -1.0) { + value = -1.0; + } + raw_outputs_[i] = value * 0.5 + 0.5; } else if (combined_output_type_[i] == M) { - write_motor(i, outputs_[i]); + if (RF_.state_manager_.state().armed) { + if (value > 1.0) { + value = 1.0; + } else if (value < RF_.params_.get_param_float(PARAM_MOTOR_IDLE_THROTTLE) + && RF_.params_.get_param_int(PARAM_SPIN_MOTORS_WHEN_ARMED)) { + value = RF_.params_.get_param_float(PARAM_MOTOR_IDLE_THROTTLE); + } else if (value < 0.0) { + value = 0.0; + } + } else { + value = 0.0; + } + raw_outputs_[i] = value; } } + RF_.board_.pwm_write_multi(raw_outputs_, NUM_TOTAL_OUTPUTS); } } // namespace rosflight_firmware diff --git a/test/test_board.cpp b/test/test_board.cpp index adfbc0e6..ba53dd90 100644 --- a/test/test_board.cpp +++ b/test/test_board.cpp @@ -154,7 +154,9 @@ float testBoard::rc_read(uint8_t channel) return static_cast(rc_values[channel] - 1000) / 1000.0; } void testBoard::pwm_write(uint8_t channel, float value) {} +void testBoard::pwm_write_multi(float * value, uint32_t channels) {} void testBoard::pwm_init(uint32_t refresh_rate, uint16_t idle_pwm) {} +void testBoard::pwm_init_multi(const float * rate, uint32_t channels) {} void testBoard::pwm_disable() {} // non-volatile memory diff --git a/test/test_board.h b/test/test_board.h index e61fa9da..5d0e19b1 100644 --- a/test/test_board.h +++ b/test/test_board.h @@ -109,8 +109,10 @@ class testBoard : public Board // PWM void pwm_init(uint32_t refresh_rate, uint16_t idle_pwm) override; + void pwm_init_multi(const float * rate, uint32_t channels) override; void pwm_disable() override; void pwm_write(uint8_t channel, float value) override; + void pwm_write_multi(float * value, uint32_t channels) override; // non-volatile memory void memory_init() override;