Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Motors: Add seperate roll and pitch limits

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;
// set limits flags
limit.roll_pitch = true;
limit.roll = true;
limit.pitch = true;
limit.yaw = true;
limit.throttle_lower = 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
// initialize limits flags
limit.roll_pitch = false;
limit.roll = false;
limit.pitch = false;
limit.yaw = false;
limit.throttle_lower = 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
// initialize limits flags
limit.roll_pitch = false;
limit.roll= false;
limit.pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
@ -493,7 +496,8 @@ void AP_Motors6DOF::output_armed_stabilizing_vectored_6dof()
float yfl_max;
// initialize limits flags
limit.roll_pitch = false;
limit.roll = false;
limit.pitch = false;
limit.yaw = false;
limit.throttle_lower = 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 {
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) {
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);
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
// 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[1] = pitch_thrust / thrust_out_actuator;
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);
}
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[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);
// override limits flags
limit.roll_pitch = true;
limit.roll = true;
limit.pitch = true;
limit.yaw = true;
limit.throttle_lower = true;
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)
{
// initialize limits flag
limit.roll_pitch = false;
limit.roll = false;
limit.pitch = false;
limit.yaw = false;
limit.throttle_lower = 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 (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) {
pitch_out = _cyclic_max/4500.0f;
limit.roll_pitch = true;
limit.pitch = true;
}
} else {
if (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) {
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)
{
// initialize limits flag
limit.roll_pitch = false;
limit.roll = false;
limit.pitch = false;
limit.yaw = false;
limit.throttle_lower = 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;
// initialize limits flag
limit.roll_pitch = false;
limit.roll = false;
limit.pitch = false;
limit.yaw = false;
limit.throttle_lower = 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;
roll_out *= ratio;
pitch_out *= ratio;
limit.roll_pitch = true;
limit.roll = true;
limit.pitch = true;
}
// 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
if (rp_high - rp_low > 1.0f || throttle_avg_max < -rp_low) {
// 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
@ -275,7 +276,8 @@ void AP_MotorsMatrix::output_armed_stabilizing()
thr_adj = throttle_thrust - throttle_thrust_best_rpy;
if (rpy_scale < 1.0f) {
// Full range is being used by roll, pitch, and yaw.
limit.roll_pitch = true;
limit.roll = true;
limit.pitch = true;
limit.yaw = true;
if (thr_adj > 0.0f) {
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) {
thr_adj = 1.0f - thrust_max;
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++) {
if (motor_enabled[i]) {
// 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.
// set limits flags
limit.roll_pitch = true;
limit.roll = true;
limit.pitch = true;
limit.yaw = true;
limit.throttle_lower = true;
limit.throttle_upper = true;
@ -543,7 +544,8 @@ void AP_MotorsMulticopter::output_logic()
// Servos should be moving to correct the current attitude.
// set limits flags
limit.roll_pitch = true;
limit.roll = true;
limit.pitch = true;
limit.yaw = true;
limit.throttle_lower = true;
limit.throttle_upper = true;
@ -587,7 +589,8 @@ void AP_MotorsMulticopter::output_logic()
// Servos should exhibit normal flight behavior.
// initialize limits flags
limit.roll_pitch = false;
limit.roll = false;
limit.pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
@ -620,7 +623,8 @@ void AP_MotorsMulticopter::output_logic()
// Servos should exhibit normal flight behavior.
// initialize limits flags
limit.roll_pitch = false;
limit.roll = false;
limit.pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
@ -647,7 +651,8 @@ void AP_MotorsMulticopter::output_logic()
// Servos should exhibit normal flight behavior.
// initialize limits flags
limit.roll_pitch = false;
limit.roll = false;
limit.pitch = false;
limit.yaw = false;
limit.throttle_lower = 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 {
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) {
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;
if (is_zero(_thrust_out)) {
limit.roll_pitch = true;
limit.roll = true;
limit.pitch = true;
limit.yaw = true;
}
@ -219,7 +221,8 @@ void AP_MotorsSingle::output_armed_stabilizing()
if (actuator_max > thrust_out_actuator && !is_zero(actuator_max)) {
// 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
limit.roll_pitch = true;
limit.roll = true;
limit.pitch = true;
limit.yaw = true;
rp_scale = thrust_out_actuator / actuator_max;
} 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) {
thr_adj = 1.0f - thrust_max;
limit.throttle_upper = true;
limit.roll_pitch = true;
limit.roll = true;
limit.pitch = true;
}
// 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;
if (rpy_scale < 1.0f) {
// 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) {
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);
// init limit flags
limit.roll_pitch = true;
limit.roll = true;
limit.pitch = true;
limit.yaw = true;
limit.throttle_lower = 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
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 throttle_lower : 1; // we have reached throttle's lower 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);
// display input and output
#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
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
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
(int)roll_in,
(int)pitch_in,
@ -171,7 +171,8 @@ void stability_test()
(int)hal.rcout->read(7),
#endif
(int)avg_out,
(int)motors.limit.roll_pitch,
(int)motors.limit.roll,
(int)motors.limit.pitch,
(int)motors.limit.yaw,
(int)motors.limit.throttle_lower,
(int)motors.limit.throttle_upper);