From 8dfbba3528930a943282906a1fc92fcaf0117b03 Mon Sep 17 00:00:00 2001 From: Jacob Moore Date: Fri, 6 Dec 2024 12:36:52 -0700 Subject: [PATCH] minor code cleanup --- include/controller.h | 1 + include/mixer.h | 2 +- src/command_manager.cpp | 2 -- src/controller.cpp | 10 +++++++--- src/mixer.cpp | 10 +++++----- 5 files changed, 14 insertions(+), 11 deletions(-) diff --git a/include/controller.h b/include/controller.h index 0d5e6a99..c997c3e9 100644 --- a/include/controller.h +++ b/include/controller.h @@ -70,6 +70,7 @@ class Controller : public ParamListenerInterface 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 diff --git a/include/mixer.h b/include/mixer.h index 405de4e7..6dc17af6 100644 --- a/include/mixer.h +++ b/include/mixer.h @@ -278,7 +278,7 @@ class Mixer : public ParamListenerInterface float R_; // Motor resistance float rho_; // Air density float K_V_; // Motor back-emf constant - float K_Q_ = 0.01706; // Motor torque constant + float K_Q_; // Motor torque constant float i_0_; // Motor no-load current float D_; // Propeller diameter float C_T_; // Thrust coefficient diff --git a/src/command_manager.cpp b/src/command_manager.cpp index 8d71db25..663ae048 100644 --- a/src/command_manager.cpp +++ b/src/command_manager.cpp @@ -114,7 +114,6 @@ void CommandManager::init_failsafe() void CommandManager::interpret_rc(void) { // get initial, unscaled RC values - // TODO: Adjust this to choose the channel that the RC thottle corresponds to 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); @@ -307,7 +306,6 @@ 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 - // TODO: Check to make sure the FX and FY commands can be set to true offboard_command_.Fx.active = false; offboard_command_.Fy.active = false; offboard_command_.Fz.active = false; diff --git a/src/controller.cpp b/src/controller.cpp index 77ac14a0..4d642017 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -101,6 +101,12 @@ void Controller::calculate_max_thrust() 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() { // Time calculation @@ -117,10 +123,8 @@ 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 - // TODO: fix this... Needs to be checked based on the throttle channel (not necessarily Fz) bool update_integrators = (RF_.state_manager_.state().armed) - && (RF_.command_manager_.combined_control().Fz.value > 0.1f) && dt_us < 10000; + && is_throttle_high(0.1f) && dt_us < 10000; // Run the PID loops Controller::Output pid_output = run_pid_loops( diff --git a/src/mixer.cpp b/src/mixer.cpp index f74fc2dd..89ac96d1 100644 --- a/src/mixer.cpp +++ b/src/mixer.cpp @@ -805,17 +805,17 @@ void Mixer::mix_fixedwing() Controller::Output commands = RF_.controller_.output(); // Reverse fixed-wing channels just before mixing if we need to - if (RF_.params_.get_param_int(PARAM_FIXED_WING)) { - 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; - } + 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] != NONE) { // 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];