aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Motors: Add seperate roll and pitch limits

Browse Source
This commit is contained in:
Leonard Hall 2019-07-27 15:07:31 +09:30 committed by Randy Mackay
parent a312249eea
commit 036b47ec56
15 changed files with 63 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_Motors/AP_Motors6DOF.cpp
View File

@ -205,7 +205,8 @@ void AP_Motors6DOF::output_min()
int8_t i; int8_t i;
// set limits flags // set limits flags
limit.roll_pitch = true; limit.roll = true;
limit.pitch = true;
limit.yaw = true; limit.yaw = true;
limit.throttle_lower = false; limit.throttle_lower = false;
limit.throttle_upper = false; limit.throttle_upper = false;
@ -302,7 +303,8 @@ void AP_Motors6DOF::output_armed_stabilizing()
float linear_out[AP_MOTORS_MAX_NUM_MOTORS]; // 3 linear DOF mix for each motor float linear_out[AP_MOTORS_MAX_NUM_MOTORS]; // 3 linear DOF mix for each motor
// initialize limits flags // initialize limits flags
limit.roll_pitch = false; limit.roll = false;
limit.pitch = false;
limit.yaw = false; limit.yaw = false;
limit.throttle_lower = false; limit.throttle_lower = false;
limit.throttle_upper = false; limit.throttle_upper = false;
@ -407,7 +409,8 @@ void AP_Motors6DOF::output_armed_stabilizing_vectored()
float linear_out[AP_MOTORS_MAX_NUM_MOTORS]; // 3 linear DOF mix for each motor float linear_out[AP_MOTORS_MAX_NUM_MOTORS]; // 3 linear DOF mix for each motor
// initialize limits flags // initialize limits flags
limit.roll_pitch = false; limit.roll= false;
limit.pitch = false;
limit.yaw = false; limit.yaw = false;
limit.throttle_lower = false; limit.throttle_lower = false;
limit.throttle_upper = false; limit.throttle_upper = false;
@ -493,7 +496,8 @@ void AP_Motors6DOF::output_armed_stabilizing_vectored_6dof()
float yfl_max; float yfl_max;
// initialize limits flags // initialize limits flags
limit.roll_pitch = false; limit.roll = false;
limit.pitch = false;
limit.yaw = false; limit.yaw = false;
limit.throttle_lower = false; limit.throttle_lower = false;
limit.throttle_upper = false; limit.throttle_upper = false;

10
libraries/AP_Motors/AP_MotorsCoax.cpp
View File

@ -163,7 +163,8 @@ void AP_MotorsCoax::output_armed_stabilizing()
} else { } else {
rp_scale = constrain_float((1.0f - MIN(fabsf(yaw_thrust), 0.5f * (float)_yaw_headroom / 1000.0f)) / rp_thrust_max, 0.0f, 1.0f); rp_scale = constrain_float((1.0f - MIN(fabsf(yaw_thrust), 0.5f * (float)_yaw_headroom / 1000.0f)) / rp_thrust_max, 0.0f, 1.0f);
if (rp_scale < 1.0f) { if (rp_scale < 1.0f) {
limit.roll_pitch = true; limit.roll = true;
limit.pitch = true;
} }
} }
@ -198,7 +199,8 @@ void AP_MotorsCoax::output_armed_stabilizing()
float thrust_out_actuator = constrain_float(MAX(_throttle_hover * 0.5f, thrust_out), 0.5f, 1.0f); float thrust_out_actuator = constrain_float(MAX(_throttle_hover * 0.5f, thrust_out), 0.5f, 1.0f);
if (is_zero(thrust_out)) { if (is_zero(thrust_out)) {
limit.roll_pitch = true; limit.roll = true;
limit.pitch = true;
} }
// force of a lifting surface is approximately equal to the angle of attack times the airflow velocity squared // force of a lifting surface is approximately equal to the angle of attack times the airflow velocity squared
// static thrust is proportional to the airflow velocity squared // static thrust is proportional to the airflow velocity squared
@ -207,11 +209,11 @@ void AP_MotorsCoax::output_armed_stabilizing()
_actuator_out[0] = roll_thrust / thrust_out_actuator; _actuator_out[0] = roll_thrust / thrust_out_actuator;
_actuator_out[1] = pitch_thrust / thrust_out_actuator; _actuator_out[1] = pitch_thrust / thrust_out_actuator;
if (fabsf(_actuator_out[0]) > 1.0f) { if (fabsf(_actuator_out[0]) > 1.0f) {
limit.roll_pitch = true; limit.roll = true;
_actuator_out[0] = constrain_float(_actuator_out[0], -1.0f, 1.0f); _actuator_out[0] = constrain_float(_actuator_out[0], -1.0f, 1.0f);
} }
if (fabsf(_actuator_out[1]) > 1.0f) { if (fabsf(_actuator_out[1]) > 1.0f) {
limit.roll_pitch = true; limit.pitch = true;
_actuator_out[1] = constrain_float(_actuator_out[1], -1.0f, 1.0f); _actuator_out[1] = constrain_float(_actuator_out[1], -1.0f, 1.0f);
} }
_actuator_out[2] = -_actuator_out[0]; _actuator_out[2] = -_actuator_out[0];

3
libraries/AP_Motors/AP_MotorsHeli.cpp
View File

@ -214,7 +214,8 @@ void AP_MotorsHeli::output_min()
update_motor_control(ROTOR_CONTROL_STOP); update_motor_control(ROTOR_CONTROL_STOP);
// override limits flags // override limits flags
limit.roll_pitch = true; limit.roll = true;
limit.pitch = true;
limit.yaw = true; limit.yaw = true;
limit.throttle_lower = true; limit.throttle_lower = true;
limit.throttle_upper = false; limit.throttle_upper = false;

11
libraries/AP_Motors/AP_MotorsHeli_Dual.cpp
View File

@ -374,7 +374,8 @@ void AP_MotorsHeli_Dual::update_motor_control(RotorControlState state)
void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float collective_in, float yaw_out) void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float collective_in, float yaw_out)
{ {
// initialize limits flag // initialize limits flag
limit.roll_pitch = false; limit.roll = false;
limit.pitch = false;
limit.yaw = false; limit.yaw = false;
limit.throttle_lower = false; limit.throttle_lower = false;
limit.throttle_upper = false; limit.throttle_upper = false;
@ -382,22 +383,22 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c
if (_dual_mode == AP_MOTORS_HELI_DUAL_MODE_TRANSVERSE) { if (_dual_mode == AP_MOTORS_HELI_DUAL_MODE_TRANSVERSE) {
if (pitch_out < -_cyclic_max/4500.0f) { if (pitch_out < -_cyclic_max/4500.0f) {
pitch_out = -_cyclic_max/4500.0f; pitch_out = -_cyclic_max/4500.0f;
limit.roll_pitch = true; limit.pitch = true;
} }
if (pitch_out > _cyclic_max/4500.0f) { if (pitch_out > _cyclic_max/4500.0f) {
pitch_out = _cyclic_max/4500.0f; pitch_out = _cyclic_max/4500.0f;
limit.roll_pitch = true; limit.pitch = true;
} }
} else { } else {
if (roll_out < -_cyclic_max/4500.0f) { if (roll_out < -_cyclic_max/4500.0f) {
roll_out = -_cyclic_max/4500.0f; roll_out = -_cyclic_max/4500.0f;
limit.roll_pitch = true; limit.roll = true;
} }
if (roll_out > _cyclic_max/4500.0f) { if (roll_out > _cyclic_max/4500.0f) {
roll_out = _cyclic_max/4500.0f; roll_out = _cyclic_max/4500.0f;
limit.roll_pitch = true; limit.roll = true;
} }
} }

3
libraries/AP_Motors/AP_MotorsHeli_Quad.cpp
View File

@ -211,7 +211,8 @@ void AP_MotorsHeli_Quad::update_motor_control(RotorControlState state)
void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float collective_in, float yaw_out) void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float collective_in, float yaw_out)
{ {
// initialize limits flag // initialize limits flag
limit.roll_pitch = false; limit.roll = false;
limit.pitch = false;
limit.yaw = false; limit.yaw = false;
limit.throttle_lower = false; limit.throttle_lower = false;
limit.throttle_upper = false; limit.throttle_upper = false;

6
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View File

@ -331,7 +331,8 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float
float yaw_offset = 0.0f; float yaw_offset = 0.0f;
// initialize limits flag // initialize limits flag
limit.roll_pitch = false; limit.roll = false;
limit.pitch = false;
limit.yaw = false; limit.yaw = false;
limit.throttle_lower = false; limit.throttle_lower = false;
limit.throttle_upper = false; limit.throttle_upper = false;
@ -350,7 +351,8 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float
float ratio = (float)(_cyclic_max/4500.0f) / total_out; float ratio = (float)(_cyclic_max/4500.0f) / total_out;
roll_out *= ratio; roll_out *= ratio;
pitch_out *= ratio; pitch_out *= ratio;
limit.roll_pitch = true; limit.roll = true;
limit.pitch = true;
} }
// constrain collective input // constrain collective input

6
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -218,7 +218,8 @@ void AP_MotorsMatrix::output_armed_stabilizing()
// check for roll and pitch saturation // check for roll and pitch saturation
if (rp_high - rp_low > 1.0f || throttle_avg_max < -rp_low) { if (rp_high - rp_low > 1.0f || throttle_avg_max < -rp_low) {
// Full range is being used by roll and pitch. // Full range is being used by roll and pitch.
limit.roll_pitch = true; limit.roll = true;
limit.pitch = true;
} }
// calculate the highest allowed average thrust that will provide maximum control range // calculate the highest allowed average thrust that will provide maximum control range
@ -275,7 +276,8 @@ void AP_MotorsMatrix::output_armed_stabilizing()
thr_adj = throttle_thrust - throttle_thrust_best_rpy; thr_adj = throttle_thrust - throttle_thrust_best_rpy;
if (rpy_scale < 1.0f) { if (rpy_scale < 1.0f) {
// Full range is being used by roll, pitch, and yaw. // Full range is being used by roll, pitch, and yaw.
limit.roll_pitch = true; limit.roll = true;
limit.pitch = true;
limit.yaw = true; limit.yaw = true;
if (thr_adj > 0.0f) { if (thr_adj > 0.0f) {
limit.throttle_upper = true; limit.throttle_upper = true;

3
libraries/AP_Motors/AP_MotorsMatrixTS.cpp
View File

@ -104,7 +104,8 @@ void AP_MotorsMatrixTS::output_armed_stabilizing()
if (thrust_max > 1.0f) { if (thrust_max > 1.0f) {
thr_adj = 1.0f - thrust_max; thr_adj = 1.0f - thrust_max;
limit.throttle_upper = true; limit.throttle_upper = true;
limit.roll_pitch = true; limit.roll = true;
limit.pitch = true;
for (int i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) { for (int i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) { if (motor_enabled[i]) {
// calculate the thrust outputs for roll and pitch // calculate the thrust outputs for roll and pitch

15
libraries/AP_Motors/AP_MotorsMulticopter.cpp
View File

@ -518,7 +518,8 @@ void AP_MotorsMulticopter::output_logic()
// Servos set to their trim values or in a test condition. // Servos set to their trim values or in a test condition.
// set limits flags // set limits flags
limit.roll_pitch = true; limit.roll = true;
limit.pitch = true;
limit.yaw = true; limit.yaw = true;
limit.throttle_lower = true; limit.throttle_lower = true;
limit.throttle_upper = true; limit.throttle_upper = true;
@ -543,7 +544,8 @@ void AP_MotorsMulticopter::output_logic()
// Servos should be moving to correct the current attitude. // Servos should be moving to correct the current attitude.
// set limits flags // set limits flags
limit.roll_pitch = true; limit.roll = true;
limit.pitch = true;
limit.yaw = true; limit.yaw = true;
limit.throttle_lower = true; limit.throttle_lower = true;
limit.throttle_upper = true; limit.throttle_upper = true;
@ -587,7 +589,8 @@ void AP_MotorsMulticopter::output_logic()
// Servos should exhibit normal flight behavior. // Servos should exhibit normal flight behavior.
// initialize limits flags // initialize limits flags
limit.roll_pitch = false; limit.roll = false;
limit.pitch = false;
limit.yaw = false; limit.yaw = false;
limit.throttle_lower = false; limit.throttle_lower = false;
limit.throttle_upper = false; limit.throttle_upper = false;
@ -620,7 +623,8 @@ void AP_MotorsMulticopter::output_logic()
// Servos should exhibit normal flight behavior. // Servos should exhibit normal flight behavior.
// initialize limits flags // initialize limits flags
limit.roll_pitch = false; limit.roll = false;
limit.pitch = false;
limit.yaw = false; limit.yaw = false;
limit.throttle_lower = false; limit.throttle_lower = false;
limit.throttle_upper = false; limit.throttle_upper = false;
@ -647,7 +651,8 @@ void AP_MotorsMulticopter::output_logic()
// Servos should exhibit normal flight behavior. // Servos should exhibit normal flight behavior.
// initialize limits flags // initialize limits flags
limit.roll_pitch = false; limit.roll = false;
limit.pitch = false;
limit.yaw = false; limit.yaw = false;
limit.throttle_lower = false; limit.throttle_lower = false;
limit.throttle_upper = false; limit.throttle_upper = false;

9
libraries/AP_Motors/AP_MotorsSingle.cpp
View File

@ -168,7 +168,8 @@ void AP_MotorsSingle::output_armed_stabilizing()
} else { } else {
rp_scale = constrain_float((1.0f - MIN(fabsf(yaw_thrust), (float) _yaw_headroom / 1000.0f)) / rp_thrust_max, 0.0f, 1.0f); rp_scale = constrain_float((1.0f - MIN(fabsf(yaw_thrust), (float) _yaw_headroom / 1000.0f)) / rp_thrust_max, 0.0f, 1.0f);
if (rp_scale < 1.0f) { if (rp_scale < 1.0f) {
limit.roll_pitch = true; limit.roll = true;
limit.pitch = true;
} }
} }
@ -203,7 +204,8 @@ void AP_MotorsSingle::output_armed_stabilizing()
_thrust_out = throttle_avg_max + thr_adj; _thrust_out = throttle_avg_max + thr_adj;
if (is_zero(_thrust_out)) { if (is_zero(_thrust_out)) {
limit.roll_pitch = true; limit.roll = true;
limit.pitch = true;
limit.yaw = true; limit.yaw = true;
} }
@ -219,7 +221,8 @@ void AP_MotorsSingle::output_armed_stabilizing()
if (actuator_max > thrust_out_actuator && !is_zero(actuator_max)) { if (actuator_max > thrust_out_actuator && !is_zero(actuator_max)) {
// roll, pitch and yaw request can not be achieved at full servo defection // roll, pitch and yaw request can not be achieved at full servo defection
// reduce roll, pitch and yaw to reduce the requested defection to maximum // reduce roll, pitch and yaw to reduce the requested defection to maximum
limit.roll_pitch = true; limit.roll = true;
limit.pitch = true;
limit.yaw = true; limit.yaw = true;
rp_scale = thrust_out_actuator / actuator_max; rp_scale = thrust_out_actuator / actuator_max;
} else { } else {

3
libraries/AP_Motors/AP_MotorsTailsitter.cpp
View File

@ -161,7 +161,8 @@ void AP_MotorsTailsitter::output_armed_stabilizing()
if (thrust_max > 1.0f) { if (thrust_max > 1.0f) {
thr_adj = 1.0f - thrust_max; thr_adj = 1.0f - thrust_max;
limit.throttle_upper = true; limit.throttle_upper = true;
limit.roll_pitch = true; limit.roll = true;
limit.pitch = true;
} }
// Add adjustment to reduce average throttle // Add adjustment to reduce average throttle

3
libraries/AP_Motors/AP_MotorsTri.cpp
View File

@ -221,7 +221,8 @@ void AP_MotorsTri::output_armed_stabilizing()
thr_adj = throttle_thrust - throttle_thrust_best_rpy; thr_adj = throttle_thrust - throttle_thrust_best_rpy;
if (rpy_scale < 1.0f) { if (rpy_scale < 1.0f) {
// Full range is being used by roll, pitch, and yaw. // Full range is being used by roll, pitch, and yaw.
limit.roll_pitch = true; limit.roll = true;
limit.pitch = true;
if (thr_adj > 0.0f) { if (thr_adj > 0.0f) {
limit.throttle_upper = true; limit.throttle_upper = true;
} }

3
libraries/AP_Motors/AP_Motors_Class.cpp
View File

@ -45,7 +45,8 @@ AP_Motors::AP_Motors(uint16_t loop_rate, uint16_t speed_hz) :
_throttle_filter.reset(0.0f); _throttle_filter.reset(0.0f);
// init limit flags // init limit flags
limit.roll_pitch = true; limit.roll = true;
limit.pitch = true;
limit.yaw = true; limit.yaw = true;
limit.throttle_lower = true; limit.throttle_lower = true;
limit.throttle_upper = true; limit.throttle_upper = true;

3
libraries/AP_Motors/AP_Motors_Class.h
View File

@ -136,7 +136,8 @@ public:
// structure for holding motor limit flags // structure for holding motor limit flags
struct AP_Motors_limit { struct AP_Motors_limit {
uint8_t roll_pitch : 1; // we have reached roll or pitch limit uint8_t roll : 1; // we have reached roll or pitch limit
uint8_t pitch : 1; // we have reached roll or pitch limit
uint8_t yaw : 1; // we have reached yaw limit uint8_t yaw : 1; // we have reached yaw limit
uint8_t throttle_lower : 1; // we have reached throttle's lower limit uint8_t throttle_lower : 1; // we have reached throttle's lower limit
uint8_t throttle_upper : 1; // we have reached throttle's upper limit uint8_t throttle_upper : 1; // we have reached throttle's upper limit

9
libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp
View File

@ -148,11 +148,11 @@ void stability_test()
avg_out = ((hal.rcout->read(0) + hal.rcout->read(1) + hal.rcout->read(2) + hal.rcout->read(3))/4); avg_out = ((hal.rcout->read(0) + hal.rcout->read(1) + hal.rcout->read(2) + hal.rcout->read(3))/4);
// display input and output // display input and output
#if NUM_OUTPUTS <= 4 #if NUM_OUTPUTS <= 4
hal.console->printf("%d,%d,%d,%3.1f,%d,%d,%d,%d,%d,%d,%d,%d,%d\n", // quad hal.console->printf("%d,%d,%d,%3.1f,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n", // quad
#elif NUM_OUTPUTS <= 6 #elif NUM_OUTPUTS <= 6
hal.console->printf("%d,%d,%d,%3.1f,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n", // hexa hal.console->printf("%d,%d,%d,%3.1f,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n", // hexa
#else #else
hal.console->printf("%d,%d,%d,%3.1f,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n", // octa hal.console->printf("%d,%d,%d,%3.1f,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n", // octa
#endif #endif
(int)roll_in, (int)roll_in,
(int)pitch_in, (int)pitch_in,
@ -171,7 +171,8 @@ void stability_test()
(int)hal.rcout->read(7), (int)hal.rcout->read(7),
#endif #endif
(int)avg_out, (int)avg_out,
(int)motors.limit.roll_pitch, (int)motors.limit.roll,
(int)motors.limit.pitch,
(int)motors.limit.yaw, (int)motors.limit.yaw,
(int)motors.limit.throttle_lower, (int)motors.limit.throttle_lower,
(int)motors.limit.throttle_upper); (int)motors.limit.throttle_upper);