From c3909c1160bd991833dbc83ea6cf77b6a9400441 Mon Sep 17 00:00:00 2001 From: Phil Tokumaru Date: Mon, 1 Jul 2024 06:47:28 -0700 Subject: [PATCH 1/5] Update to support per PWM output rate, vector servo update, and DSHOT frequencies (and protocol) Also fixed a missing check for 'M' --- include/board.h | 2 + include/mixer.h | 274 +++++++++++++++++++++++++----------------------- src/mixer.cpp | 52 ++++++--- 3 files changed, 179 insertions(+), 149 deletions(-) diff --git a/include/board.h b/include/board.h index 2b350b97..8d17c38a 100644 --- a/include/board.h +++ b/include/board.h @@ -116,8 +116,10 @@ class Board // PWM virtual void pwm_init(uint32_t refresh_rate, uint16_t idle_pwm) = 0; + virtual void pwm_init(const float *rate, uint32_t channels) = 0; // PTT new virtual void pwm_disable() = 0; virtual void pwm_write(uint8_t channel, float value) = 0; + virtual void pwm_write(float *value, uint32_t channels) = 0; //PTT new // non-volatile memory virtual void memory_init() = 0; diff --git a/include/mixer.h b/include/mixer.h index d11359b6..3bc89505 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,8 +62,9 @@ class Mixer : public ParamListenerInterface FIXEDWING = 10, PASSTHROUGH = 11, VTAIL = 12, - CUSTOM = 13, - NUM_MIXERS, + QUADPLANE = 13, + CUSTOM = 14, + NUM_MIXERS, INVALID_MIXER = 255 }; @@ -78,12 +79,12 @@ 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; + } 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}, - - {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 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}; - - const mixer_t hex_plus_mixing = { - {M, M, M, M, M, M, M, M}, // output_type - - {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // F Mix - {0.0f, -0.866025f, -0.866025f, 0.0f, 0.866025f, 0.866025f, 0.0f, 0.0f}, // X Mix - {1.0f, 0.5f, -0.5f, -1.0f, -0.5f, 0.5f, 0.0f, 0.0f}, // Y Mix - {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f}, // Z Mix - 490}; - - const mixer_t hex_x_mixing = { - {M, M, M, M, M, M, M, M}, // output_type - - {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // F Mix - {-0.5f, -1.0f, -0.5f, 0.5f, 1.0f, 0.5f, 0.0f, 0.0f}, // X Mix - {0.866025f, 0.0f, -0.866025f, -0.866025f, 0.0f, 0.866025f, 0.0f, 0.0f}, // Y Mix - {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f}, // Z Mix - 490}; - - const mixer_t octocopter_plus_mixing = { - {M, M, M, M, M, M, M, M}, // output_type - - {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f}, // F Mix - {0.0f, -0.707f, -1.0f, -0.707f, 0.0f, 0.707f, 1.0f, 0.707f}, // X Mix - {1.0f, 0.707f, 0.0f, -0.707f, -1.0f, -0.707f, 0.0f, 0.707f}, // Y Mix - {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f}, // Z Mix - 490}; - - const mixer_t octocopter_x_mixing = { - {M, M, M, M, M, M, M, M}, // output_type - - {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f}, // F Mix - {-0.414f, -1.0f, -1.0f, -0.414f, 0.414f, 1.0f, 1.0f, 0.414}, // X Mix - {1.0f, 0.414f, -0.414f, -1.0f, -1.0f, -0.414f, 0.414f, 1.0}, // Y Mix - {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f}, // Z Mix - 490}; - - const mixer_t Y6_mixing = { - {M, M, M, M, M, M, NONE, NONE}, // output_type - - {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // F Mix - {-1.0f, -1.0f, 0.0f, 0.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // X Mix - {0.667f, 0.667f, -1.333f, -1.333f, 0.667f, 0.667f, 0.0f, 0.0f}, // Y Mix - {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f}, // Z Mix - 490}; - - const mixer_t X8_mixing = {{M, M, M, M, M, M, M, M}, // output_type + // clang-format off - {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}; + const mixer_t esc_calibration_mixing = { // + {M, M, M, M, M, M, NONE, NONE, NONE, NONE}, // + { 50, 50, 50, 50, 50, 50, 50, 50, 50, 50}, // Rate (Hz or kHz) + {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // F Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // X Mix + + const mixer_t quadcopter_plus_mixing = { // + {M, M, M, M, NONE, NONE, NONE, NONE, NONE, NONE}, // output_type + {3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5}, // Rate (Hz) + { 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix + { 0.0f, -1.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + { 1.0f, 0.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + { 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix + + const mixer_t quadcopter_x_mixing = { // + {M, M, M, M, NONE, NONE, NONE, NONE}, // output_type + {3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5}, // Rate (Hz) + {1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix + {-1.0f, -1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {1.0f, -1.0f, -1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + {1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix + + const mixer_t hex_plus_mixing = { // + {M, M, M, M, M, M, M, M, NONE, NONE}, // output_type + {3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5}, // Rate (Hz) + {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix + {0.0f, -0.866025f, -0.866025f, 0.0f, 0.866025f, 0.866025f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {1.0f, 0.5f, -0.5f, -1.0f, -0.5f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix + + const mixer_t hex_x_mixing = { // + {M, M, M, M, M, M, M, M, NONE, NONE}, // output_type + {3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5}, // Rate (Hz) + {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix + {-0.5f, -1.0f, -0.5f, 0.5f, 1.0f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {0.866025f, 0.0f, -0.866025f, -0.866025f, 0.0f, 0.866025f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f} }; // Z Mix + + const mixer_t octocopter_plus_mixing = { // + {M, M, M, M, M, M, M, M, NONE, NONE}, // output_type + {3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5}, // Rate (Hz) + {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // F Mix + {0.0f, -0.707f, -1.0f, -0.707f, 0.0f, 0.707f, 1.0f, 0.707f, 0.0f, 0.0f}, // X Mix + {1.0f, 0.707f, 0.0f, -0.707f, -1.0f, -0.707f, 0.0f, 0.707f, 0.0f, 0.0f}, // Y Mix + {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f}}; // Z Mix + + const mixer_t octocopter_x_mixing = { // + {M, M, M, M, M, M, M, M, NONE, NONE}, // output_type + {3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5}, // Rate (Hz) + {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // F Mix + {-0.414f, -1.0f, -1.0f, -0.414f, 0.414f, 1.0f, 1.0f, 0.414, 0.0f, 0.0f}, // X Mix + {1.0f, 0.414f, -0.414f, -1.0f, -1.0f, -0.414f, 0.414f, 1.0, 0.0f, 0.0f}, // Y Mix + {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f}}; // Z Mix + + const mixer_t Y6_mixing = { // + {M, M, M, M, M, M, NONE, NONE, NONE, NONE}, // output_type + {3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5}, // Rate (Hz) + {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix + {-1.0f, -1.0f, 0.0f, 0.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {0.667f, 0.667f, -1.333f, -1.333f, 0.667f, 0.667f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix + + const mixer_t X8_mixing = { // + {M, M, M, M, M, M, M, M, NONE, NONE}, // output_type + {3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5}, // Rate (Hz) + {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // F Mix + {-1.0f, -1.0f, -1.0f, -1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // X Mix + {1.0f, 1.0f, -1.0f, -1.0f, -1.0f, -1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // Y Mix + {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f} };// Z Mix const mixer_t tricopter_mixing = { - {M, M, M, S, NONE, NONE, NONE, NONE}, // output_type - - {1.0f, 0.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix - {-1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - {0.667f, 0.0f, 0.667f, -1.333f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix - {0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Z Mix - 490}; - - const mixer_t fixedwing_mixing = {{S, S, M, S, S, M, NONE, NONE}, // output type - - {0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix - {1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - {0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix - {0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Z Mix - 50}; - - const mixer_t passthrough_mixing = {{NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE}, - - {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix - {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix - {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Z Mix - 50}; + { M, M, M, NONE, S, NONE, NONE, NONE, NONE, NONE}, // output_type + { 3e5, 3e5, 3e5, 3e5, 3e5, 50, 50, 50, 50, 50}, // Rate (Hz or kHz) + { 1.000f, 0.000f, 1.000f, 0.000f, 1.000f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix + {-1.000f, 0.000f, 0.000f, 0.000f, 1.000f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + { 0.667f, 0.000f, 0.667f, 0.000f, -1.333f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + { 0.000f, 1.000f, 0.000f, 0.000f, 0.000f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f} }; // Z Mix + + const mixer_t fixedwing_mixing = { + { S, S, S, NONE, M, NONE, NONE, NONE, NONE, NONE}, // output type + { 50, 50, 50, 50, 50, 50, 50, 50, 50, 50}, // Rate (Hz or kHz) + {0.0f, 0.0f, 1.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix + {1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + {0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix const mixer_t fixedwing_vtail_mixing = { - {S, S, M, S, NONE, NONE, NONE, NONE}, // Ailerons, LRuddervator, Motor, RRuddervator - - {0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix - {1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - {0.0f, -0.5f, 0.0f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix - {0.0f, 0.5f, 0.0f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f}, // Z Mix - 50}; - - const mixer_t custom_mixing = {{NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE}, // output type - - {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix - {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix - {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Z Mix - 50}; + { S, S, S, NONE, M, NONE, NONE, NONE, NONE, NONE}, // Ailerons, LRuddervator, RRuddervator, Motor + { 50, 50, 50, 50, 50, 50, 50, 50, 50, 50}, // Rate (Hz or kHz) + {0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix + {1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {0.0f, -0.5f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + {0.0f, 0.5f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix + + const mixer_t quadplane_mixing = { + { S, S, S, M, M, M, M, M, NONE, NONE}, // Ailerons, Rudder, Elevator, Tractor Motor, Quadrotors + { 50, 50, 50, 50, 3e5, 3e5, 3e5, 3e5, 50, 50}, // Rate (Hz or kHz) + {0.0f, 0.0f, 0.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // F Mix + {1.0f, 0.0f, 0.0f, 0.0f, 0.0f,-1.0f, 0.0f, 1.0f, 0.0f, 0.0f}, // X Mix + {0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f,-1.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + {0.0f, 0.0f, 1.0f, 0.0f, 1.0f,-1.0f, 1.0f,-1.0f, 0.0f, 0.0f}}; // Z Mix + + const mixer_t passthrough_mixing = { // + {NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE}, + { 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..41c8cb22 100644 --- a/src/mixer.cpp +++ b/src/mixer.cpp @@ -88,15 +88,10 @@ 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; - } - 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); + if(mixer_to_use_ != nullptr) + RF_.board_.pwm_init( mixer_to_use_->default_pwm_rate, NUM_MIXER_OUTPUTS); + else + RF_.board_.pwm_init( passthrough_mixing.default_pwm_rate, NUM_MIXER_OUTPUTS); } void Mixer::write_motor(uint8_t index, float value) @@ -172,7 +167,8 @@ 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) @@ -195,13 +191,39 @@ void Mixer::mix_output() } // Write to outputs - for (uint8_t i = 0; i < NUM_TOTAL_OUTPUTS; i++) { - if (combined_output_type_[i] == S) { - write_servo(i, outputs_[i]); - } else if (combined_output_type_[i] == M) { - write_motor(i, outputs_[i]); + for (uint8_t i = 0; i < NUM_TOTAL_OUTPUTS; i++) { + float value=outputs_[i]; + if (combined_output_type_[i] == S) // + { + // PTT removed 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) // + { + //PTT removed 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(raw_outputs_, NUM_TOTAL_OUTPUTS); + } } // namespace rosflight_firmware From c562f81d0c746adf6e2a27c64fcd8251b817428d Mon Sep 17 00:00:00 2001 From: Phil Tokumaru Date: Mon, 1 Jul 2024 07:01:41 -0700 Subject: [PATCH 2/5] Added Multi-Servo Frequency and Updates --- include/board.h | 4 ++-- src/mixer.cpp | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/include/board.h b/include/board.h index 8d17c38a..59b354cc 100644 --- a/include/board.h +++ b/include/board.h @@ -116,10 +116,10 @@ class Board // PWM virtual void pwm_init(uint32_t refresh_rate, uint16_t idle_pwm) = 0; - virtual void pwm_init(const float *rate, uint32_t channels) = 0; // PTT new + virtual void pwm_init_multi(const float *rate, uint32_t channels) = 0; // PTT new virtual void pwm_disable() = 0; virtual void pwm_write(uint8_t channel, float value) = 0; - virtual void pwm_write(float *value, uint32_t channels) = 0; //PTT new + virtual void pwm_write_multi(float *value, uint32_t channels) = 0; //PTT new // non-volatile memory virtual void memory_init() = 0; diff --git a/src/mixer.cpp b/src/mixer.cpp index 41c8cb22..6cfcc702 100644 --- a/src/mixer.cpp +++ b/src/mixer.cpp @@ -89,9 +89,9 @@ void Mixer::init_mixing() void Mixer::init_PWM() { if(mixer_to_use_ != nullptr) - RF_.board_.pwm_init( mixer_to_use_->default_pwm_rate, NUM_MIXER_OUTPUTS); + RF_.board_.pwm_init_multi( mixer_to_use_->default_pwm_rate, NUM_MIXER_OUTPUTS); else - RF_.board_.pwm_init( passthrough_mixing.default_pwm_rate, NUM_MIXER_OUTPUTS); + RF_.board_.pwm_init_multi( passthrough_mixing.default_pwm_rate, NUM_MIXER_OUTPUTS); } void Mixer::write_motor(uint8_t index, float value) @@ -222,7 +222,7 @@ void Mixer::mix_output() } } - RF_.board_.pwm_write(raw_outputs_, NUM_TOTAL_OUTPUTS); + RF_.board_.pwm_write_multi(raw_outputs_, NUM_TOTAL_OUTPUTS); } From c22051d1161dc14d69ddbcee703b51c346280c87 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Mon, 1 Jul 2024 15:04:15 -0600 Subject: [PATCH 3/5] update varmint_h7 module for PWM changes --- boards/varmint_h7 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 From 437a652ac8bf5d3fd66fca019bb4b8542b7ac34d Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Mon, 1 Jul 2024 15:09:33 -0600 Subject: [PATCH 4/5] added new pwm board.h function to test board --- test/test_board.cpp | 2 ++ test/test_board.h | 2 ++ 2 files changed, 4 insertions(+) diff --git a/test/test_board.cpp b/test/test_board.cpp index adfbc0e6..0e39cdc2 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..fd1ed0d9 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; From 99031a57a39f3409924bbf31cc9d4496c7d2e744 Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Wed, 3 Jul 2024 13:01:33 -0600 Subject: [PATCH 5/5] Formatting changes --- include/board.h | 4 +- include/mixer.h | 254 ++++++++++++++++++++++---------------------- src/mixer.cpp | 67 ++++++------ test/test_board.cpp | 4 +- test/test_board.h | 4 +- 5 files changed, 163 insertions(+), 170 deletions(-) diff --git a/include/board.h b/include/board.h index 59b354cc..15dbe117 100644 --- a/include/board.h +++ b/include/board.h @@ -116,10 +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; // PTT new + 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; //PTT new + 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 3bc89505..7a0b7a13 100644 --- a/include/mixer.h +++ b/include/mixer.h @@ -62,9 +62,9 @@ class Mixer : public ParamListenerInterface FIXEDWING = 10, PASSTHROUGH = 11, VTAIL = 12, - QUADPLANE = 13, + QUADPLANE = 13, CUSTOM = 14, - NUM_MIXERS, + NUM_MIXERS, INVALID_MIXER = 255 }; @@ -84,7 +84,7 @@ class Mixer : public ParamListenerInterface float x[NUM_MIXER_OUTPUTS]; float y[NUM_MIXER_OUTPUTS]; float z[NUM_MIXER_OUTPUTS]; - } mixer_t; + } mixer_t; typedef struct { @@ -110,146 +110,146 @@ class Mixer : public ParamListenerInterface // clang-format off - const mixer_t esc_calibration_mixing = { // - {M, M, M, M, M, M, NONE, NONE, NONE, NONE}, // - { 50, 50, 50, 50, 50, 50, 50, 50, 50, 50}, // Rate (Hz or kHz) - {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // F Mix - {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // X Mix - - const mixer_t quadcopter_plus_mixing = { // - {M, M, M, M, NONE, NONE, NONE, NONE, NONE, NONE}, // output_type - {3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5}, // Rate (Hz) - { 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix - { 0.0f, -1.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - { 1.0f, 0.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix - { 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix - - const mixer_t quadcopter_x_mixing = { // - {M, M, M, M, NONE, NONE, NONE, NONE}, // output_type - {3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5}, // Rate (Hz) - {1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix - {-1.0f, -1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - {1.0f, -1.0f, -1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix - {1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix - - const mixer_t hex_plus_mixing = { // - {M, M, M, M, M, M, M, M, NONE, NONE}, // output_type - {3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5}, // Rate (Hz) - {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix - {0.0f, -0.866025f, -0.866025f, 0.0f, 0.866025f, 0.866025f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - {1.0f, 0.5f, -0.5f, -1.0f, -0.5f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix - {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix - - const mixer_t hex_x_mixing = { // - {M, M, M, M, M, M, M, M, NONE, NONE}, // output_type - {3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5}, // Rate (Hz) - {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix - {-0.5f, -1.0f, -0.5f, 0.5f, 1.0f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - {0.866025f, 0.0f, -0.866025f, -0.866025f, 0.0f, 0.866025f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix - {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f} }; // Z Mix - - const mixer_t octocopter_plus_mixing = { // - {M, M, M, M, M, M, M, M, NONE, NONE}, // output_type - {3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5}, // Rate (Hz) - {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // F Mix - {0.0f, -0.707f, -1.0f, -0.707f, 0.0f, 0.707f, 1.0f, 0.707f, 0.0f, 0.0f}, // X Mix - {1.0f, 0.707f, 0.0f, -0.707f, -1.0f, -0.707f, 0.0f, 0.707f, 0.0f, 0.0f}, // Y Mix - {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f}}; // Z Mix - - const mixer_t octocopter_x_mixing = { // - {M, M, M, M, M, M, M, M, NONE, NONE}, // output_type - {3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5}, // Rate (Hz) - {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // F Mix - {-0.414f, -1.0f, -1.0f, -0.414f, 0.414f, 1.0f, 1.0f, 0.414, 0.0f, 0.0f}, // X Mix - {1.0f, 0.414f, -0.414f, -1.0f, -1.0f, -0.414f, 0.414f, 1.0, 0.0f, 0.0f}, // Y Mix - {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f}}; // Z Mix - - const mixer_t Y6_mixing = { // - {M, M, M, M, M, M, NONE, NONE, NONE, NONE}, // output_type - {3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5}, // Rate (Hz) - {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix - {-1.0f, -1.0f, 0.0f, 0.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - {0.667f, 0.667f, -1.333f, -1.333f, 0.667f, 0.667f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix - {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix - - const mixer_t X8_mixing = { // - {M, M, M, M, M, M, M, M, NONE, NONE}, // output_type - {3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5, 3e5}, // Rate (Hz) - {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // F Mix - {-1.0f, -1.0f, -1.0f, -1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // X Mix - {1.0f, 1.0f, -1.0f, -1.0f, -1.0f, -1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // Y Mix - {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f} };// Z Mix + const mixer_t 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, 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, 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, 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, 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, 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, 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, NONE, S, NONE, NONE, NONE, NONE, NONE}, // output_type - { 3e5, 3e5, 3e5, 3e5, 3e5, 50, 50, 50, 50, 50}, // Rate (Hz or kHz) - { 1.000f, 0.000f, 1.000f, 0.000f, 1.000f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix - {-1.000f, 0.000f, 0.000f, 0.000f, 1.000f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - { 0.667f, 0.000f, 0.667f, 0.000f, -1.333f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix - { 0.000f, 1.000f, 0.000f, 0.000f, 0.000f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f} }; // Z Mix + { 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 or kHz) - {0.0f, 0.0f, 1.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix - {1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix - {0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix - {0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix + { 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, S, NONE, M, NONE, NONE, NONE, NONE, NONE}, // Ailerons, LRuddervator, RRuddervator, Motor - { 50, 50, 50, 50, 50, 50, 50, 50, 50, 50}, // Rate (Hz or kHz) + { 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, 3e5, 3e5, 3e5, 3e5, 50, 50}, // Rate (Hz or kHz) - {0.0f, 0.0f, 0.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // F Mix - {1.0f, 0.0f, 0.0f, 0.0f, 0.0f,-1.0f, 0.0f, 1.0f, 0.0f, 0.0f}, // X Mix - {0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f,-1.0f, 0.0f, 0.0f, 0.0f}, // Y Mix - {0.0f, 0.0f, 1.0f, 0.0f, 1.0f,-1.0f, 1.0f,-1.0f, 0.0f, 0.0f}}; // Z Mix - - const mixer_t passthrough_mixing = { // - {NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE}, - { 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 + { 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_; - 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 - }; + 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 6cfcc702..1115eb7a 100644 --- a/src/mixer.cpp +++ b/src/mixer.cpp @@ -88,10 +88,11 @@ void Mixer::init_mixing() void Mixer::init_PWM() { - 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); + 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); + } } void Mixer::write_motor(uint8_t index, float value) @@ -167,8 +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) - if (mixer_to_use_->output_type[i] == M) - 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) @@ -191,39 +191,32 @@ 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) // - { - // PTT removed 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) // - { - //PTT removed 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; + for (uint8_t i = 0; i < NUM_TOTAL_OUTPUTS; i++) { + float value = outputs_[i]; + if (combined_output_type_[i] == S) { + 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) { + 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); - + 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 0e39cdc2..ba53dd90 100644 --- a/test/test_board.cpp +++ b/test/test_board.cpp @@ -154,9 +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_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_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 fd1ed0d9..5d0e19b1 100644 --- a/test/test_board.h +++ b/test/test_board.h @@ -109,10 +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_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; + void pwm_write_multi(float * value, uint32_t channels) override; // non-volatile memory void memory_init() override;