From dc39f77da609af5bb532c72793caad9d02e21002 Mon Sep 17 00:00:00 2001 From: Junichiro Sugihara Date: Thu, 17 Aug 2023 14:00:38 +0900 Subject: [PATCH] [Spinal][Controller] modified the condition for updating torque allocation matrix. --- .../lib/Jsk_Lib/flight_control/attitude/attitude_control.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.cpp index be6a220a1..57b8add1f 100644 --- a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.cpp +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.cpp @@ -585,7 +585,7 @@ void AttitudeController::rpyGainCallback( const spinal::RollPitchYawTerms &gain_ void AttitudeController::torqueAllocationMatrixInvCallback(const spinal::TorqueAllocationMatrixInv& msg) { - if(motor_number_ == 0) return; + if(motor_number_ == 0 || !start_control_flag_) return; #ifdef SIMULATION if(msg.rows.size() != motor_number_) @@ -947,7 +947,7 @@ void AttitudeController::pwmConversion() } } } - + for(int i = 0; i < motor_number_; i++) target_thrust_[i] = roll_pitch_term_[i] + (1 + base_thrust_decreasing_rate) * base_thrust_term_[i] + (1 + yaw_decreasing_rate) * yaw_term_[i];