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

AP_Motors: formatting fixes

This commit is contained in:
Leonard Hall 2019-04-20 10:29:40 +09:30 committed by Randy Mackay
parent 4321a987f2
commit 77f8ec9f43
8 changed files with 283 additions and 288 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AP_Motors/AP_MotorsCoax.cpp
View File

@ -26,13 +26,12 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
// init // init
void AP_MotorsCoax::init(motor_frame_class frame_class, motor_frame_type frame_type) void AP_MotorsCoax::init(motor_frame_class frame_class, motor_frame_type frame_type)
{ {
// make sure 6 output channels are mapped // make sure 6 output channels are mapped
for (uint8_t i=0; i<6; i++) { for (uint8_t i = 0; i < 6; i++) {
add_motor_num(CH_1+i); add_motor_num(CH_1 + i);
} }
// set the motor_enabled flag so that the main ESC can be calibrated like other frame types // set the motor_enabled flag so that the main ESC can be calibrated like other frame types
@ -40,10 +39,10 @@ void AP_MotorsCoax::init(motor_frame_class frame_class, motor_frame_type frame_t
motor_enabled[AP_MOTORS_MOT_6] = true; motor_enabled[AP_MOTORS_MOT_6] = true;
// setup actuator scaling // setup actuator scaling
for (uint8_t i=0; i<NUM_ACTUATORS; i++) { for (uint8_t i = 0; i < NUM_ACTUATORS; i++) {
SRV_Channels::set_angle(SRV_Channels::get_motor_function(i), AP_MOTORS_COAX_SERVO_INPUT_RANGE); SRV_Channels::set_angle(SRV_Channels::get_motor_function(i), AP_MOTORS_COAX_SERVO_INPUT_RANGE);
} }
// record successful initialisation if what we setup was the desired frame_class // record successful initialisation if what we setup was the desired frame_class
_flags.initialised_ok = (frame_class == MOTOR_FRAME_COAX); _flags.initialised_ok = (frame_class == MOTOR_FRAME_COAX);
} }
@ -55,7 +54,7 @@ void AP_MotorsCoax::set_frame_class_and_type(motor_frame_class frame_class, moto
} }
// set update rate to motors - a value in hertz // set update rate to motors - a value in hertz
void AP_MotorsCoax::set_update_rate( uint16_t speed_hz ) void AP_MotorsCoax::set_update_rate(uint16_t speed_hz)
{ {
// record requested speed // record requested speed
_speed_hz = speed_hz; _speed_hz = speed_hz;
@ -80,8 +79,8 @@ void AP_MotorsCoax::output_to_motors()
break; break;
case SpoolState::GROUND_IDLE: case SpoolState::GROUND_IDLE:
// sends output to motors when armed but not flying // sends output to motors when armed but not flying
for (uint8_t i=0; i<NUM_ACTUATORS; i++) { for (uint8_t i = 0; i < NUM_ACTUATORS; i++) {
rc_write_angle(AP_MOTORS_MOT_1+i, _spin_up_ratio * _actuator_out[i] * AP_MOTORS_COAX_SERVO_INPUT_RANGE); rc_write_angle(AP_MOTORS_MOT_1 + i, _spin_up_ratio * _actuator_out[i] * AP_MOTORS_COAX_SERVO_INPUT_RANGE);
} }
set_actuator_with_slew(_actuator[5], actuator_spin_up_to_ground_idle()); set_actuator_with_slew(_actuator[5], actuator_spin_up_to_ground_idle());
set_actuator_with_slew(_actuator[6], actuator_spin_up_to_ground_idle()); set_actuator_with_slew(_actuator[6], actuator_spin_up_to_ground_idle());
@ -92,8 +91,8 @@ void AP_MotorsCoax::output_to_motors()
case SpoolState::THROTTLE_UNLIMITED: case SpoolState::THROTTLE_UNLIMITED:
case SpoolState::SPOOLING_DOWN: case SpoolState::SPOOLING_DOWN:
// set motor output based on thrust requests // set motor output based on thrust requests
for (uint8_t i=0; i<NUM_ACTUATORS; i++) { for (uint8_t i = 0; i < NUM_ACTUATORS; i++) {
rc_write_angle(AP_MOTORS_MOT_1+i, _actuator_out[i] * AP_MOTORS_COAX_SERVO_INPUT_RANGE); rc_write_angle(AP_MOTORS_MOT_1 + i, _actuator_out[i] * AP_MOTORS_COAX_SERVO_INPUT_RANGE);
} }
set_actuator_with_slew(_actuator[5], thrust_to_actuator(_thrust_yt_ccw)); set_actuator_with_slew(_actuator[5], thrust_to_actuator(_thrust_yt_ccw));
set_actuator_with_slew(_actuator[6], thrust_to_actuator(_thrust_yt_cw)); set_actuator_with_slew(_actuator[6], thrust_to_actuator(_thrust_yt_cw));
@ -162,7 +161,7 @@ void AP_MotorsCoax::output_armed_stabilizing()
if (is_zero(rp_thrust_max)) { if (is_zero(rp_thrust_max)) {
rp_scale = 1.0f; rp_scale = 1.0f;
} 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_pitch = true;
} }
@ -196,7 +195,7 @@ void AP_MotorsCoax::output_armed_stabilizing()
_thrust_yt_cw = thrust_out - 0.5f * yaw_thrust; _thrust_yt_cw = thrust_out - 0.5f * yaw_thrust;
// limit thrust out for calculation of actuator gains // limit thrust out for calculation of actuator gains
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_pitch = true;
@ -205,8 +204,8 @@ void AP_MotorsCoax::output_armed_stabilizing()
// static thrust is proportional to the airflow velocity squared // static thrust is proportional to the airflow velocity squared
// therefore the torque of the roll and pitch actuators should be approximately proportional to // therefore the torque of the roll and pitch actuators should be approximately proportional to
// the angle of attack multiplied by the static thrust. // the angle of attack multiplied by the static thrust.
_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_pitch = 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);

79
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -38,18 +38,18 @@ void AP_MotorsMatrix::init(motor_frame_class frame_class, motor_frame_type frame
} }
// set update rate to motors - a value in hertz // set update rate to motors - a value in hertz
void AP_MotorsMatrix::set_update_rate( uint16_t speed_hz ) void AP_MotorsMatrix::set_update_rate(uint16_t speed_hz)
{ {
// record requested speed // record requested speed
_speed_hz = speed_hz; _speed_hz = speed_hz;
uint16_t mask = 0; uint16_t mask = 0;
for (uint8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) { for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) { if (motor_enabled[i]) {
mask |= 1U << i; mask |= 1U << i;
} }
} }
rc_set_freq(mask, _speed_hz ); rc_set_freq(mask, _speed_hz);
} }
// set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus) // set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus)
@ -76,7 +76,7 @@ void AP_MotorsMatrix::output_to_motors()
switch (_spool_state) { switch (_spool_state) {
case SpoolState::SHUT_DOWN: { case SpoolState::SHUT_DOWN: {
// no output // no output
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) { for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) { if (motor_enabled[i]) {
_actuator[i] = 0.0f; _actuator[i] = 0.0f;
} }
@ -85,7 +85,7 @@ void AP_MotorsMatrix::output_to_motors()
} }
case SpoolState::GROUND_IDLE: case SpoolState::GROUND_IDLE:
// sends output to motors when armed but not flying // sends output to motors when armed but not flying
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) { for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) { if (motor_enabled[i]) {
set_actuator_with_slew(_actuator[i], actuator_spin_up_to_ground_idle()); set_actuator_with_slew(_actuator[i], actuator_spin_up_to_ground_idle());
} }
@ -95,7 +95,7 @@ void AP_MotorsMatrix::output_to_motors()
case SpoolState::THROTTLE_UNLIMITED: case SpoolState::THROTTLE_UNLIMITED:
case SpoolState::SPOOLING_DOWN: case SpoolState::SPOOLING_DOWN:
// set motor output based on thrust requests // set motor output based on thrust requests
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) { for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) { if (motor_enabled[i]) {
set_actuator_with_slew(_actuator[i], thrust_to_actuator(_thrust_rpyt_out[i])); set_actuator_with_slew(_actuator[i], thrust_to_actuator(_thrust_rpyt_out[i]));
} }
@ -104,20 +104,19 @@ void AP_MotorsMatrix::output_to_motors()
} }
// convert output to PWM and send to each motor // convert output to PWM and send to each motor
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) { for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) { if (motor_enabled[i]) {
rc_write(i, output_to_pwm(_actuator[i])); rc_write(i, output_to_pwm(_actuator[i]));
} }
} }
} }
// get_motor_mask - returns a bitmask of which outputs are being used for motors (1 means being used) // get_motor_mask - returns a bitmask of which outputs are being used for motors (1 means being used)
// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict // this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
uint16_t AP_MotorsMatrix::get_motor_mask() uint16_t AP_MotorsMatrix::get_motor_mask()
{ {
uint16_t motor_mask = 0; uint16_t motor_mask = 0;
for (uint8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) { for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) { if (motor_enabled[i]) {
motor_mask |= 1U << i; motor_mask |= 1U << i;
} }
@ -193,15 +192,15 @@ void AP_MotorsMatrix::output_armed_stabilizing()
// this is always equal to or less than the requested yaw from the pilot or rate controller // this is always equal to or less than the requested yaw from the pilot or rate controller
float rp_low = 1.0f; // lowest thrust value float rp_low = 1.0f; // lowest thrust value
float rp_high = -1.0f; // highest thrust value float rp_high = -1.0f; // highest thrust value
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) { for (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
_thrust_rpyt_out[i] = roll_thrust * _roll_factor[i] + pitch_thrust * _pitch_factor[i]; _thrust_rpyt_out[i] = roll_thrust * _roll_factor[i] + pitch_thrust * _pitch_factor[i];
// record lowest roll+pitch command // record lowest roll + pitch command
if (_thrust_rpyt_out[i] < rp_low) { if (_thrust_rpyt_out[i] < rp_low) {
rp_low = _thrust_rpyt_out[i]; rp_low = _thrust_rpyt_out[i];
} }
// record highest roll+pitch command // record highest roll + pitch command
if (_thrust_rpyt_out[i] > rp_high && (!_thrust_boost || i != _motor_lost_index)) { if (_thrust_rpyt_out[i] > rp_high && (!_thrust_boost || i != _motor_lost_index)) {
rp_high = _thrust_rpyt_out[i]; rp_high = _thrust_rpyt_out[i];
} }
@ -210,14 +209,14 @@ void AP_MotorsMatrix::output_armed_stabilizing()
// include the lost motor scaled by _thrust_boost_ratio // include the lost motor scaled by _thrust_boost_ratio
if (_thrust_boost && motor_enabled[_motor_lost_index]) { if (_thrust_boost && motor_enabled[_motor_lost_index]) {
// record highest roll+pitch command // record highest roll + pitch command
if (_thrust_rpyt_out[_motor_lost_index] > rp_high) { if (_thrust_rpyt_out[_motor_lost_index] > rp_high) {
rp_high = _thrust_boost_ratio*rp_high + (1.0f-_thrust_boost_ratio)*_thrust_rpyt_out[_motor_lost_index]; rp_high = _thrust_boost_ratio * rp_high + (1.0f - _thrust_boost_ratio) * _thrust_rpyt_out[_motor_lost_index];
} }
} }
// 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_pitch = true;
} }
@ -228,8 +227,8 @@ void AP_MotorsMatrix::output_armed_stabilizing()
// calculate the maximum yaw control that can be used // calculate the maximum yaw control that can be used
// todo: make _yaw_headroom 0 to 1 // todo: make _yaw_headroom 0 to 1
yaw_allowed = (float)_yaw_headroom / 1000.0f; yaw_allowed = (float)_yaw_headroom / 1000.0f;
yaw_allowed = _thrust_boost_ratio*0.5f + (1.0f - _thrust_boost_ratio) * yaw_allowed; yaw_allowed = _thrust_boost_ratio * 0.5f + (1.0f - _thrust_boost_ratio) * yaw_allowed;
yaw_allowed = MAX(MIN(throttle_thrust_best_rpy+rp_low, 1.0f - (throttle_thrust_best_rpy + rp_high)), yaw_allowed); yaw_allowed = MAX(MIN(throttle_thrust_best_rpy + rp_low, 1.0f - (throttle_thrust_best_rpy + rp_high)), yaw_allowed);
if (fabsf(yaw_thrust) > yaw_allowed) { if (fabsf(yaw_thrust) > yaw_allowed) {
// not all commanded yaw can be used // not all commanded yaw can be used
yaw_thrust = constrain_float(yaw_thrust, -yaw_allowed, yaw_allowed); yaw_thrust = constrain_float(yaw_thrust, -yaw_allowed, yaw_allowed);
@ -239,15 +238,15 @@ void AP_MotorsMatrix::output_armed_stabilizing()
// add yaw control to thrust outputs // add yaw control to thrust outputs
float rpy_low = 1.0f; // lowest thrust value float rpy_low = 1.0f; // lowest thrust value
float rpy_high = -1.0f; // highest thrust value float rpy_high = -1.0f; // highest thrust value
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) { for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) { if (motor_enabled[i]) {
_thrust_rpyt_out[i] = _thrust_rpyt_out[i] + yaw_thrust * _yaw_factor[i]; _thrust_rpyt_out[i] = _thrust_rpyt_out[i] + yaw_thrust * _yaw_factor[i];
// record lowest roll+pitch+yaw command // record lowest roll + pitch + yaw command
if (_thrust_rpyt_out[i] < rpy_low) { if (_thrust_rpyt_out[i] < rpy_low) {
rpy_low = _thrust_rpyt_out[i]; rpy_low = _thrust_rpyt_out[i];
} }
// record highest roll+pitch+yaw command // record highest roll + pitch + yaw command
if (_thrust_rpyt_out[i] > rpy_high && (!_thrust_boost || i != _motor_lost_index)) { if (_thrust_rpyt_out[i] > rpy_high && (!_thrust_boost || i != _motor_lost_index)) {
rpy_high = _thrust_rpyt_out[i]; rpy_high = _thrust_rpyt_out[i];
} }
@ -255,15 +254,15 @@ void AP_MotorsMatrix::output_armed_stabilizing()
} }
// include the lost motor scaled by _thrust_boost_ratio // include the lost motor scaled by _thrust_boost_ratio
if (_thrust_boost) { if (_thrust_boost) {
// record highest roll+pitch+yaw command // record highest roll + pitch + yaw command
if (_thrust_rpyt_out[_motor_lost_index] > rpy_high && motor_enabled[_motor_lost_index]) { if (_thrust_rpyt_out[_motor_lost_index] > rpy_high && motor_enabled[_motor_lost_index]) {
rpy_high = _thrust_boost_ratio*rpy_high + (1.0f-_thrust_boost_ratio)*_thrust_rpyt_out[_motor_lost_index]; rpy_high = _thrust_boost_ratio * rpy_high + (1.0f - _thrust_boost_ratio) * _thrust_rpyt_out[_motor_lost_index];
} }
} }
// calculate any scaling needed to make the combined thrust outputs fit within the output range // calculate any scaling needed to make the combined thrust outputs fit within the output range
if (rpy_high-rpy_low > 1.0f) { if (rpy_high - rpy_low > 1.0f) {
rpy_scale = 1.0f / (rpy_high-rpy_low); rpy_scale = 1.0f / (rpy_high - rpy_low);
} }
if (is_negative(rpy_low)) { if (is_negative(rpy_low)) {
rpy_scale = MIN(rpy_scale, -throttle_avg_max / rpy_low); rpy_scale = MIN(rpy_scale, -throttle_avg_max / rpy_low);
@ -295,7 +294,7 @@ void AP_MotorsMatrix::output_armed_stabilizing()
} }
// add scaled roll, pitch, constrained yaw and throttle for each motor // add scaled roll, pitch, constrained yaw and throttle for each motor
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) { for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) { if (motor_enabled[i]) {
_thrust_rpyt_out[i] = throttle_thrust_best_rpy + thr_adj + (rpy_scale * _thrust_rpyt_out[i]); _thrust_rpyt_out[i] = throttle_thrust_best_rpy + thr_adj + (rpy_scale * _thrust_rpyt_out[i]);
} }
@ -370,7 +369,7 @@ void AP_MotorsMatrix::output_test_seq(uint8_t motor_seq, int16_t pwm)
} }
// loop through all the possible orders spinning any motors that match that description // loop through all the possible orders spinning any motors that match that description
for (uint8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) { for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i] && _test_order[i] == motor_seq) { if (motor_enabled[i] && _test_order[i] == motor_seq) {
// turn on this motor // turn on this motor
rc_write(i, pwm); rc_write(i, pwm);
@ -390,7 +389,7 @@ bool AP_MotorsMatrix::output_test_num(uint8_t output_channel, int16_t pwm)
} }
// Is channel in supported range? // Is channel in supported range?
if (output_channel > AP_MOTORS_MAX_NUM_MOTORS -1) { if (output_channel > AP_MOTORS_MAX_NUM_MOTORS - 1) {
return false; return false;
} }
@ -407,10 +406,10 @@ bool AP_MotorsMatrix::output_test_num(uint8_t output_channel, int16_t pwm)
void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, uint8_t testing_order) void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, uint8_t testing_order)
{ {
// ensure valid motor number is provided // ensure valid motor number is provided
if( motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS ) { if (motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS) {
// increment number of motors if this motor is being newly motor_enabled // increment number of motors if this motor is being newly motor_enabled
if( !motor_enabled[motor_num] ) { if (!motor_enabled[motor_num]) {
motor_enabled[motor_num] = true; motor_enabled[motor_num] = true;
} }
@ -448,7 +447,7 @@ void AP_MotorsMatrix::add_motor(int8_t motor_num, float roll_factor_in_degrees,
void AP_MotorsMatrix::remove_motor(int8_t motor_num) void AP_MotorsMatrix::remove_motor(int8_t motor_num)
{ {
// ensure valid motor number is provided // ensure valid motor number is provided
if( motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS ) { if (motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS) {
// disable the motor, set all factors to zero // disable the motor, set all factors to zero
motor_enabled[motor_num] = false; motor_enabled[motor_num] = false;
_roll_factor[motor_num] = 0; _roll_factor[motor_num] = 0;
@ -460,7 +459,7 @@ void AP_MotorsMatrix::remove_motor(int8_t motor_num)
void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_type frame_type) void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_type frame_type)
{ {
// remove existing motors // remove existing motors
for (int8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) { for (int8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
remove_motor(i); remove_motor(i);
} }
@ -561,10 +560,10 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
break; break;
case MOTOR_FRAME_TYPE_PLUSREV: case MOTOR_FRAME_TYPE_PLUSREV:
// plus with reversed motor directions // plus with reversed motor directions
add_motor(AP_MOTORS_MOT_1, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2); add_motor(AP_MOTORS_MOT_1, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4); add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
add_motor(AP_MOTORS_MOT_3, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,1); add_motor(AP_MOTORS_MOT_3, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
add_motor(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW,3); add_motor(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
break; break;
default: default:
// quad frame class does not support this frame type // quad frame class does not support this frame type
@ -788,7 +787,7 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
// matrix doesn't support the configured class // matrix doesn't support the configured class
success = false; success = false;
break; break;
} // switch frame_class } // switch frame_class
// normalise factors to magnitude 0.5 // normalise factors to magnitude 0.5
normalise_rpy_factors(); normalise_rpy_factors();
@ -804,7 +803,7 @@ void AP_MotorsMatrix::normalise_rpy_factors()
float yaw_fac = 0.0f; float yaw_fac = 0.0f;
// find maximum roll, pitch and yaw factors // find maximum roll, pitch and yaw factors
for (uint8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) { for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) { if (motor_enabled[i]) {
if (roll_fac < fabsf(_roll_factor[i])) { if (roll_fac < fabsf(_roll_factor[i])) {
roll_fac = fabsf(_roll_factor[i]); roll_fac = fabsf(_roll_factor[i]);
@ -819,16 +818,16 @@ void AP_MotorsMatrix::normalise_rpy_factors()
} }
// scale factors back to -0.5 to +0.5 for each axis // scale factors back to -0.5 to +0.5 for each axis
for (uint8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) { for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) { if (motor_enabled[i]) {
if (!is_zero(roll_fac)) { if (!is_zero(roll_fac)) {
_roll_factor[i] = 0.5f*_roll_factor[i]/roll_fac; _roll_factor[i] = 0.5f * _roll_factor[i] / roll_fac;
} }
if (!is_zero(pitch_fac)) { if (!is_zero(pitch_fac)) {
_pitch_factor[i] = 0.5f*_pitch_factor[i]/pitch_fac; _pitch_factor[i] = 0.5f * _pitch_factor[i] / pitch_fac;
} }
if (!is_zero(yaw_fac)) { if (!is_zero(yaw_fac)) {
_yaw_factor[i] = 0.5f*_yaw_factor[i]/yaw_fac; _yaw_factor[i] = 0.5f * _yaw_factor[i] / yaw_fac;
} }
} }
} }

358
libraries/AP_Motors/AP_MotorsMulticopter.cpp
View File

@ -208,9 +208,9 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = {
// Constructor // Constructor
AP_MotorsMulticopter::AP_MotorsMulticopter(uint16_t loop_rate, uint16_t speed_hz) : AP_MotorsMulticopter::AP_MotorsMulticopter(uint16_t loop_rate, uint16_t speed_hz) :
AP_Motors(loop_rate, speed_hz), AP_Motors(loop_rate, speed_hz),
_lift_max(1.0f), _lift_max(1.0f),
_throttle_limit(1.0f) _throttle_limit(1.0f)
{ {
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
@ -240,7 +240,7 @@ void AP_MotorsMulticopter::output()
// apply any thrust compensation for the frame // apply any thrust compensation for the frame
thrust_compensation(); thrust_compensation();
// convert rpy_thrust values to pwm // convert rpy_thrust values to pwm
output_to_motors(); output_to_motors();
@ -253,10 +253,9 @@ void AP_MotorsMulticopter::output_boost_throttle(void)
{ {
if (_boost_scale > 0) { if (_boost_scale > 0) {
float throttle = constrain_float(get_throttle() * _boost_scale, 0, 1); float throttle = constrain_float(get_throttle() * _boost_scale, 0, 1);
SRV_Channels::set_output_scaled(SRV_Channel::k_boost_throttle, throttle*1000); SRV_Channels::set_output_scaled(SRV_Channel::k_boost_throttle, throttle * 1000);
} }
} }
// sends minimum values out to the motors // sends minimum values out to the motors
void AP_MotorsMulticopter::output_min() void AP_MotorsMulticopter::output_min()
@ -270,7 +269,7 @@ void AP_MotorsMulticopter::output_min()
void AP_MotorsMulticopter::update_throttle_filter() void AP_MotorsMulticopter::update_throttle_filter()
{ {
if (armed()) { if (armed()) {
_throttle_filter.apply(_throttle_in, 1.0f/_loop_rate); _throttle_filter.apply(_throttle_in, 1.0f / _loop_rate);
// constrain filtered throttle // constrain filtered throttle
if (_throttle_filter.get() < 0.0f) { if (_throttle_filter.get() < 0.0f) {
_throttle_filter.reset(0.0f); _throttle_filter.reset(0.0f);
@ -305,18 +304,18 @@ float AP_MotorsMulticopter::get_current_limit_max_throttle()
float _batt_current = battery.current_amps(_batt_idx); float _batt_current = battery.current_amps(_batt_idx);
// calculate the maximum current to prevent voltage sag below _batt_voltage_min // calculate the maximum current to prevent voltage sag below _batt_voltage_min
float batt_current_max = MIN(_batt_current_max, _batt_current + (battery.voltage(_batt_idx)-_batt_voltage_min)/_batt_resistance); float batt_current_max = MIN(_batt_current_max, _batt_current + (battery.voltage(_batt_idx) - _batt_voltage_min) / _batt_resistance);
float batt_current_ratio = _batt_current/batt_current_max; float batt_current_ratio = _batt_current / batt_current_max;
float loop_interval = 1.0f/_loop_rate; float loop_interval = 1.0f / _loop_rate;
_throttle_limit += (loop_interval/(loop_interval+_batt_current_time_constant))*(1.0f - batt_current_ratio); _throttle_limit += (loop_interval / (loop_interval + _batt_current_time_constant)) * (1.0f - batt_current_ratio);
// throttle limit drops to 20% between hover and full throttle // throttle limit drops to 20% between hover and full throttle
_throttle_limit = constrain_float(_throttle_limit, 0.2f, 1.0f); _throttle_limit = constrain_float(_throttle_limit, 0.2f, 1.0f);
// limit max throttle // limit max throttle
return get_throttle_hover() + ((1.0-get_throttle_hover())*_throttle_limit); return get_throttle_hover() + ((1.0 - get_throttle_hover()) * _throttle_limit);
} }
// apply_thrust_curve_and_volt_scaling - returns throttle in the range 0 ~ 1 // apply_thrust_curve_and_volt_scaling - returns throttle in the range 0 ~ 1
@ -329,10 +328,10 @@ float AP_MotorsMulticopter::apply_thrust_curve_and_volt_scaling(float thrust) co
// zero expo means linear, avoid floating point exception for small values // zero expo means linear, avoid floating point exception for small values
return thrust; return thrust;
} }
if(!is_zero(_batt_voltage_filt.get())) { if (!is_zero(_batt_voltage_filt.get())) {
throttle_ratio = ((thrust_curve_expo-1.0f) + safe_sqrt((1.0f-thrust_curve_expo)*(1.0f-thrust_curve_expo) + 4.0f*thrust_curve_expo*_lift_max*thrust))/(2.0f*thrust_curve_expo*_batt_voltage_filt.get()); throttle_ratio = ((thrust_curve_expo - 1.0f) + safe_sqrt((1.0f - thrust_curve_expo) * (1.0f - thrust_curve_expo) + 4.0f * thrust_curve_expo * _lift_max * thrust)) / (2.0f * thrust_curve_expo * _batt_voltage_filt.get());
} else { } else {
throttle_ratio = ((thrust_curve_expo-1.0f) + safe_sqrt((1.0f-thrust_curve_expo)*(1.0f-thrust_curve_expo) + 4.0f*thrust_curve_expo*_lift_max*thrust))/(2.0f*thrust_curve_expo); throttle_ratio = ((thrust_curve_expo - 1.0f) + safe_sqrt((1.0f - thrust_curve_expo) * (1.0f - thrust_curve_expo) + 4.0f * thrust_curve_expo * _lift_max * thrust)) / (2.0f * thrust_curve_expo);
} }
return constrain_float(throttle_ratio, 0.0f, 1.0f); return constrain_float(throttle_ratio, 0.0f, 1.0f);
@ -344,7 +343,7 @@ void AP_MotorsMulticopter::update_lift_max_from_batt_voltage()
// sanity check battery_voltage_min is not too small // sanity check battery_voltage_min is not too small
// if disabled or misconfigured exit immediately // if disabled or misconfigured exit immediately
float _batt_voltage_resting_estimate = AP::battery().voltage_resting_estimate(_batt_idx); float _batt_voltage_resting_estimate = AP::battery().voltage_resting_estimate(_batt_idx);
if((_batt_voltage_max <= 0) || (_batt_voltage_min >= _batt_voltage_max) || (_batt_voltage_resting_estimate < 0.25f*_batt_voltage_min)) { if ((_batt_voltage_max <= 0) || (_batt_voltage_min >= _batt_voltage_max) || (_batt_voltage_resting_estimate < 0.25f * _batt_voltage_min)) {
_batt_voltage_filt.reset(1.0f); _batt_voltage_filt.reset(1.0f);
_lift_max = 1.0f; _lift_max = 1.0f;
return; return;
@ -356,11 +355,11 @@ void AP_MotorsMulticopter::update_lift_max_from_batt_voltage()
_batt_voltage_resting_estimate = constrain_float(_batt_voltage_resting_estimate, _batt_voltage_min, _batt_voltage_max); _batt_voltage_resting_estimate = constrain_float(_batt_voltage_resting_estimate, _batt_voltage_min, _batt_voltage_max);
// filter at 0.5 Hz // filter at 0.5 Hz
float batt_voltage_filt = _batt_voltage_filt.apply(_batt_voltage_resting_estimate/_batt_voltage_max, 1.0f/_loop_rate); float batt_voltage_filt = _batt_voltage_filt.apply(_batt_voltage_resting_estimate / _batt_voltage_max, 1.0f / _loop_rate);
// calculate lift max // calculate lift max
float thrust_curve_expo = constrain_float(_thrust_curve_expo, -1.0f, 1.0f); float thrust_curve_expo = constrain_float(_thrust_curve_expo, -1.0f, 1.0f);
_lift_max = batt_voltage_filt*(1-thrust_curve_expo) + thrust_curve_expo*batt_voltage_filt*batt_voltage_filt; _lift_max = batt_voltage_filt * (1 - thrust_curve_expo) + thrust_curve_expo * batt_voltage_filt * batt_voltage_filt;
} }
float AP_MotorsMulticopter::get_compensation_gain() const float AP_MotorsMulticopter::get_compensation_gain() const
@ -375,7 +374,7 @@ float AP_MotorsMulticopter::get_compensation_gain() const
#if AP_MOTORS_DENSITY_COMP == 1 #if AP_MOTORS_DENSITY_COMP == 1
// air density ratio is increasing in density / decreasing in altitude // air density ratio is increasing in density / decreasing in altitude
if (_air_density_ratio > 0.3f && _air_density_ratio < 1.5f) { if (_air_density_ratio > 0.3f && _air_density_ratio < 1.5f) {
ret *= 1.0f / constrain_float(_air_density_ratio,0.5f,1.25f); ret *= 1.0f / constrain_float(_air_density_ratio, 0.5f, 1.25f);
} }
#endif #endif
return ret; return ret;
@ -394,7 +393,7 @@ int16_t AP_MotorsMulticopter::output_to_pwm(float actuator)
} }
} else { } else {
// in all other spool modes, covert to desired PWM // in all other spool modes, covert to desired PWM
pwm_output = get_pwm_output_min() + (get_pwm_output_max()-get_pwm_output_min()) * actuator; pwm_output = get_pwm_output_min() + (get_pwm_output_max() - get_pwm_output_min()) * actuator;
} }
return pwm_output; return pwm_output;
@ -404,7 +403,7 @@ int16_t AP_MotorsMulticopter::output_to_pwm(float actuator)
float AP_MotorsMulticopter::thrust_to_actuator(float thrust_in) float AP_MotorsMulticopter::thrust_to_actuator(float thrust_in)
{ {
thrust_in = constrain_float(thrust_in, 0.0f, 1.0f); thrust_in = constrain_float(thrust_in, 0.0f, 1.0f);
return _spin_min + (_spin_max-_spin_min)*apply_thrust_curve_and_volt_scaling(thrust_in); return _spin_min + (_spin_max - _spin_min) * apply_thrust_curve_and_volt_scaling(thrust_in);
} }
// adds slew rate limiting to actuator output // adds slew rate limiting to actuator output
@ -423,13 +422,13 @@ void AP_MotorsMulticopter::set_actuator_with_slew(float& actuator_output, float
// If MOT_SLEW_UP_TIME is set, calculate the highest allowed new output value, constrained 0.0~1.0 // If MOT_SLEW_UP_TIME is set, calculate the highest allowed new output value, constrained 0.0~1.0
if (is_positive(_slew_up_time)) { if (is_positive(_slew_up_time)) {
float output_delta_up_max = 1.0f/(constrain_float(_slew_up_time,0.0f,0.5f) * _loop_rate); float output_delta_up_max = 1.0f / (constrain_float(_slew_up_time, 0.0f, 0.5f) * _loop_rate);
output_slew_limit_up = constrain_float(actuator_output + output_delta_up_max, 0.0f, 1.0f); output_slew_limit_up = constrain_float(actuator_output + output_delta_up_max, 0.0f, 1.0f);
} }
// If MOT_SLEW_DN_TIME is set, calculate the lowest allowed new output value, constrained 0.0~1.0 // If MOT_SLEW_DN_TIME is set, calculate the lowest allowed new output value, constrained 0.0~1.0
if (is_positive(_slew_dn_time)) { if (is_positive(_slew_dn_time)) {
float output_delta_dn_max = 1.0f/(constrain_float(_slew_dn_time,0.0f,0.5f) * _loop_rate); float output_delta_dn_max = 1.0f / (constrain_float(_slew_dn_time, 0.0f, 0.5f) * _loop_rate);
output_slew_limit_dn = constrain_float(actuator_output - output_delta_dn_max, 0.0f, 1.0f); output_slew_limit_dn = constrain_float(actuator_output - output_delta_dn_max, 0.0f, 1.0f);
} }
@ -489,7 +488,7 @@ void AP_MotorsMulticopter::update_throttle_hover(float dt)
{ {
if (_throttle_hover_learn != HOVER_LEARN_DISABLED) { if (_throttle_hover_learn != HOVER_LEARN_DISABLED) {
// we have chosen to constrain the hover throttle to be within the range reachable by the third order expo polynomial. // we have chosen to constrain the hover throttle to be within the range reachable by the third order expo polynomial.
_throttle_hover = constrain_float(_throttle_hover + (dt/(dt+AP_MOTORS_THST_HOVER_TC))*(get_throttle()-_throttle_hover), AP_MOTORS_THST_HOVER_MIN, AP_MOTORS_THST_HOVER_MAX); _throttle_hover = constrain_float(_throttle_hover + (dt / (dt + AP_MOTORS_THST_HOVER_TC)) * (get_throttle() - _throttle_hover), AP_MOTORS_THST_HOVER_MIN, AP_MOTORS_THST_HOVER_MAX);
} }
} }
@ -514,167 +513,167 @@ void AP_MotorsMulticopter::output_logic()
} }
switch (_spool_state) { switch (_spool_state) {
case SpoolState::SHUT_DOWN: case SpoolState::SHUT_DOWN:
// Motors should be stationary. // Motors should be stationary.
// 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_pitch = true;
limit.yaw = true; limit.yaw = true;
limit.throttle_lower = true; limit.throttle_lower = true;
limit.throttle_upper = true; limit.throttle_upper = true;
// make sure the motors are spooling in the correct direction // make sure the motors are spooling in the correct direction
if (_spool_desired != DesiredSpoolState::SHUT_DOWN) { if (_spool_desired != DesiredSpoolState::SHUT_DOWN) {
_spool_state = SpoolState::GROUND_IDLE; _spool_state = SpoolState::GROUND_IDLE;
break;
}
// set and increment ramp variables
_spin_up_ratio = 0.0f;
_throttle_thrust_max = 0.0f;
// initialise motor failure variables
_thrust_boost = false;
_thrust_boost_ratio = 0.0f;
break;
case SpoolState::GROUND_IDLE: {
// Motors should be stationary or at ground idle.
// Servos should be moving to correct the current attitude.
// set limits flags
limit.roll_pitch = true;
limit.yaw = true;
limit.throttle_lower = true;
limit.throttle_upper = true;
// set and increment ramp variables
float spool_step = 1.0f/(_spool_up_time*_loop_rate);
switch (_spool_desired) {
case DesiredSpoolState::SHUT_DOWN:
_spin_up_ratio -= spool_step;
// constrain ramp value and update mode
if (_spin_up_ratio <= 0.0f) {
_spin_up_ratio = 0.0f;
_spool_state = SpoolState::SHUT_DOWN;
}
break;
case DesiredSpoolState::THROTTLE_UNLIMITED:
_spin_up_ratio += spool_step;
// constrain ramp value and update mode
if (_spin_up_ratio >= 1.0f) {
_spin_up_ratio = 1.0f;
_spool_state = SpoolState::SPOOLING_UP;
}
break;
case DesiredSpoolState::GROUND_IDLE:
float spin_up_armed_ratio = 0.0f;
if (_spin_min > 0.0f) {
spin_up_armed_ratio = _spin_arm / _spin_min;
}
_spin_up_ratio += constrain_float(spin_up_armed_ratio-_spin_up_ratio, -spool_step, spool_step);
break;
}
_throttle_thrust_max = 0.0f;
// initialise motor failure variables
_thrust_boost = false;
_thrust_boost_ratio = 0.0f;
break; break;
} }
case SpoolState::SPOOLING_UP:
// Maximum throttle should move from minimum to maximum.
// Servos should exhibit normal flight behavior.
// initialize limits flags // set and increment ramp variables
limit.roll_pitch = false; _spin_up_ratio = 0.0f;
limit.yaw = false; _throttle_thrust_max = 0.0f;
limit.throttle_lower = false;
limit.throttle_upper = false;
// make sure the motors are spooling in the correct direction // initialise motor failure variables
if (_spool_desired != DesiredSpoolState::THROTTLE_UNLIMITED ){ _thrust_boost = false;
_spool_state = SpoolState::SPOOLING_DOWN; _thrust_boost_ratio = 0.0f;
break; break;
}
// set and increment ramp variables case SpoolState::GROUND_IDLE: {
_spin_up_ratio = 1.0f; // Motors should be stationary or at ground idle.
_throttle_thrust_max += 1.0f/(_spool_up_time*_loop_rate); // Servos should be moving to correct the current attitude.
// set limits flags
limit.roll_pitch = true;
limit.yaw = true;
limit.throttle_lower = true;
limit.throttle_upper = true;
// set and increment ramp variables
float spool_step = 1.0f / (_spool_up_time * _loop_rate);
switch (_spool_desired) {
case DesiredSpoolState::SHUT_DOWN:
_spin_up_ratio -= spool_step;
// constrain ramp value and update mode // constrain ramp value and update mode
if (_throttle_thrust_max >= MIN(get_throttle(), get_current_limit_max_throttle())) { if (_spin_up_ratio <= 0.0f) {
_throttle_thrust_max = get_current_limit_max_throttle(); _spin_up_ratio = 0.0f;
_spool_state = SpoolState::THROTTLE_UNLIMITED; _spool_state = SpoolState::SHUT_DOWN;
} else if (_throttle_thrust_max < 0.0f) {
_throttle_thrust_max = 0.0f;
}
// initialise motor failure variables
_thrust_boost = false;
_thrust_boost_ratio = MAX(0.0, _thrust_boost_ratio - 1.0 / (_spool_up_time * _loop_rate));
break;
case SpoolState::THROTTLE_UNLIMITED:
// Throttle should exhibit normal flight behavior.
// Servos should exhibit normal flight behavior.
// initialize limits flags
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
// make sure the motors are spooling in the correct direction
if (_spool_desired != DesiredSpoolState::THROTTLE_UNLIMITED) {
_spool_state = SpoolState::SPOOLING_DOWN;
break;
}
// set and increment ramp variables
_spin_up_ratio = 1.0f;
_throttle_thrust_max = get_current_limit_max_throttle();
if (_thrust_boost && !_thrust_balanced) {
_thrust_boost_ratio = MIN(1.0, _thrust_boost_ratio+1.0f/(_spool_up_time*_loop_rate));
} else {
_thrust_boost_ratio = MAX(0.0, _thrust_boost_ratio-1.0f/(_spool_up_time*_loop_rate));
} }
break; break;
case DesiredSpoolState::THROTTLE_UNLIMITED:
case SpoolState::SPOOLING_DOWN: _spin_up_ratio += spool_step;
// Maximum throttle should move from maximum to minimum. // constrain ramp value and update mode
// Servos should exhibit normal flight behavior. if (_spin_up_ratio >= 1.0f) {
_spin_up_ratio = 1.0f;
// initialize limits flags
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
// make sure the motors are spooling in the correct direction
if (_spool_desired == DesiredSpoolState::THROTTLE_UNLIMITED) {
_spool_state = SpoolState::SPOOLING_UP; _spool_state = SpoolState::SPOOLING_UP;
break;
} }
// set and increment ramp variables
_spin_up_ratio = 1.0f;
_throttle_thrust_max -= 1.0f/(_spool_up_time*_loop_rate);
// constrain ramp value and update mode
if (_throttle_thrust_max <= 0.0f){
_throttle_thrust_max = 0.0f;
}
if (_throttle_thrust_max >= get_current_limit_max_throttle()) {
_throttle_thrust_max = get_current_limit_max_throttle();
} else if (is_zero(_throttle_thrust_max)) {
_spool_state = SpoolState::GROUND_IDLE;
}
_thrust_boost_ratio = MAX(0.0, _thrust_boost_ratio-1.0f/(_spool_up_time*_loop_rate));
break; break;
case DesiredSpoolState::GROUND_IDLE:
float spin_up_armed_ratio = 0.0f;
if (_spin_min > 0.0f) {
spin_up_armed_ratio = _spin_arm / _spin_min;
}
_spin_up_ratio += constrain_float(spin_up_armed_ratio - _spin_up_ratio, -spool_step, spool_step);
break;
}
_throttle_thrust_max = 0.0f;
// initialise motor failure variables
_thrust_boost = false;
_thrust_boost_ratio = 0.0f;
break;
}
case SpoolState::SPOOLING_UP:
// Maximum throttle should move from minimum to maximum.
// Servos should exhibit normal flight behavior.
// initialize limits flags
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
// make sure the motors are spooling in the correct direction
if (_spool_desired != DesiredSpoolState::THROTTLE_UNLIMITED) {
_spool_state = SpoolState::SPOOLING_DOWN;
break;
}
// set and increment ramp variables
_spin_up_ratio = 1.0f;
_throttle_thrust_max += 1.0f / (_spool_up_time * _loop_rate);
// constrain ramp value and update mode
if (_throttle_thrust_max >= MIN(get_throttle(), get_current_limit_max_throttle())) {
_throttle_thrust_max = get_current_limit_max_throttle();
_spool_state = SpoolState::THROTTLE_UNLIMITED;
} else if (_throttle_thrust_max < 0.0f) {
_throttle_thrust_max = 0.0f;
}
// initialise motor failure variables
_thrust_boost = false;
_thrust_boost_ratio = MAX(0.0, _thrust_boost_ratio - 1.0 / (_spool_up_time * _loop_rate));
break;
case SpoolState::THROTTLE_UNLIMITED:
// Throttle should exhibit normal flight behavior.
// Servos should exhibit normal flight behavior.
// initialize limits flags
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
// make sure the motors are spooling in the correct direction
if (_spool_desired != DesiredSpoolState::THROTTLE_UNLIMITED) {
_spool_state = SpoolState::SPOOLING_DOWN;
break;
}
// set and increment ramp variables
_spin_up_ratio = 1.0f;
_throttle_thrust_max = get_current_limit_max_throttle();
if (_thrust_boost && !_thrust_balanced) {
_thrust_boost_ratio = MIN(1.0, _thrust_boost_ratio + 1.0f / (_spool_up_time * _loop_rate));
} else {
_thrust_boost_ratio = MAX(0.0, _thrust_boost_ratio - 1.0f / (_spool_up_time * _loop_rate));
}
break;
case SpoolState::SPOOLING_DOWN:
// Maximum throttle should move from maximum to minimum.
// Servos should exhibit normal flight behavior.
// initialize limits flags
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
// make sure the motors are spooling in the correct direction
if (_spool_desired == DesiredSpoolState::THROTTLE_UNLIMITED) {
_spool_state = SpoolState::SPOOLING_UP;
break;
}
// set and increment ramp variables
_spin_up_ratio = 1.0f;
_throttle_thrust_max -= 1.0f / (_spool_up_time * _loop_rate);
// constrain ramp value and update mode
if (_throttle_thrust_max <= 0.0f) {
_throttle_thrust_max = 0.0f;
}
if (_throttle_thrust_max >= get_current_limit_max_throttle()) {
_throttle_thrust_max = get_current_limit_max_throttle();
} else if (is_zero(_throttle_thrust_max)) {
_spool_state = SpoolState::GROUND_IDLE;
}
_thrust_boost_ratio = MAX(0.0, _thrust_boost_ratio - 1.0f / (_spool_up_time * _loop_rate));
break;
} }
} }
@ -685,7 +684,7 @@ void AP_MotorsMulticopter::set_throttle_passthrough_for_esc_calibration(float th
if (armed()) { if (armed()) {
uint16_t pwm_out = get_pwm_output_min() + constrain_float(throttle_input, 0.0f, 1.0f) * (get_pwm_output_max() - get_pwm_output_min()); uint16_t pwm_out = get_pwm_output_min() + constrain_float(throttle_input, 0.0f, 1.0f) * (get_pwm_output_max() - get_pwm_output_min());
// send the pilot's input directly to each enabled motor // send the pilot's input directly to each enabled motor
for (uint16_t i=0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) { for (uint16_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) { if (motor_enabled[i]) {
rc_write(i, pwm_out); rc_write(i, pwm_out);
} }
@ -701,18 +700,17 @@ void AP_MotorsMulticopter::set_throttle_passthrough_for_esc_calibration(float th
// the range 0 to 1 // the range 0 to 1
void AP_MotorsMulticopter::output_motor_mask(float thrust, uint8_t mask, float rudder_dt) void AP_MotorsMulticopter::output_motor_mask(float thrust, uint8_t mask, float rudder_dt)
{ {
for (uint8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) { for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) { if (motor_enabled[i]) {
if (mask & (1U<<i)) { if (mask & (1U << i)) {
/* /*
apply rudder mixing differential thrust apply rudder mixing differential thrust
copter frame roll is plane frame yaw as this only copter frame roll is plane frame yaw as this only
apples to either tilted motors or tailsitters apples to either tilted motors or tailsitters
*/ */
float diff_thrust = get_roll_factor(i) * rudder_dt * 0.5f; float diff_thrust = get_roll_factor(i) * rudder_dt * 0.5f;
set_actuator_with_slew(_actuator[i], thrust_to_actuator(thrust + diff_thrust)); set_actuator_with_slew(_actuator[i], thrust_to_actuator(thrust + diff_thrust));
int16_t pwm_output = get_pwm_output_min() + int16_t pwm_output = get_pwm_output_min() + (get_pwm_output_max() - get_pwm_output_min()) * _actuator[i];
(get_pwm_output_max()-get_pwm_output_min()) * _actuator[i];
rc_write(i, pwm_output); rc_write(i, pwm_output);
} else { } else {
rc_write(i, get_pwm_output_min()); rc_write(i, get_pwm_output_min());

33
libraries/AP_Motors/AP_MotorsSingle.cpp
View File

@ -26,13 +26,12 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
// init // init
void AP_MotorsSingle::init(motor_frame_class frame_class, motor_frame_type frame_type) void AP_MotorsSingle::init(motor_frame_class frame_class, motor_frame_type frame_type)
{ {
// make sure 6 output channels are mapped // make sure 6 output channels are mapped
for (uint8_t i=0; i<6; i++) { for (uint8_t i = 0; i < 6; i++) {
add_motor_num(CH_1+i); add_motor_num(CH_1 + i);
} }
// set the motor_enabled flag so that the main ESC can be calibrated like other frame types // set the motor_enabled flag so that the main ESC can be calibrated like other frame types
@ -40,7 +39,7 @@ void AP_MotorsSingle::init(motor_frame_class frame_class, motor_frame_type frame
motor_enabled[AP_MOTORS_MOT_6] = true; motor_enabled[AP_MOTORS_MOT_6] = true;
// setup actuator scaling // setup actuator scaling
for (uint8_t i=0; i<NUM_ACTUATORS; i++) { for (uint8_t i = 0; i < NUM_ACTUATORS; i++) {
SRV_Channels::set_angle(SRV_Channels::get_motor_function(i), AP_MOTORS_SINGLE_SERVO_INPUT_RANGE); SRV_Channels::set_angle(SRV_Channels::get_motor_function(i), AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
} }
@ -55,7 +54,7 @@ void AP_MotorsSingle::set_frame_class_and_type(motor_frame_class frame_class, mo
} }
// set update rate to motors - a value in hertz // set update rate to motors - a value in hertz
void AP_MotorsSingle::set_update_rate( uint16_t speed_hz ) void AP_MotorsSingle::set_update_rate(uint16_t speed_hz)
{ {
// record requested speed // record requested speed
_speed_hz = speed_hz; _speed_hz = speed_hz;
@ -83,8 +82,8 @@ void AP_MotorsSingle::output_to_motors()
break; break;
case SpoolState::GROUND_IDLE: case SpoolState::GROUND_IDLE:
// sends output to motors when armed but not flying // sends output to motors when armed but not flying
for (uint8_t i=0; i<NUM_ACTUATORS; i++) { for (uint8_t i = 0; i < NUM_ACTUATORS; i++) {
rc_write_angle(AP_MOTORS_MOT_1+i, _spin_up_ratio * _actuator_out[i] * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE); rc_write_angle(AP_MOTORS_MOT_1 + i, _spin_up_ratio * _actuator_out[i] * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
} }
set_actuator_with_slew(_actuator[5], actuator_spin_up_to_ground_idle()); set_actuator_with_slew(_actuator[5], actuator_spin_up_to_ground_idle());
set_actuator_with_slew(_actuator[6], actuator_spin_up_to_ground_idle()); set_actuator_with_slew(_actuator[6], actuator_spin_up_to_ground_idle());
@ -95,8 +94,8 @@ void AP_MotorsSingle::output_to_motors()
case SpoolState::THROTTLE_UNLIMITED: case SpoolState::THROTTLE_UNLIMITED:
case SpoolState::SPOOLING_DOWN: case SpoolState::SPOOLING_DOWN:
// set motor output based on thrust requests // set motor output based on thrust requests
for (uint8_t i=0; i<NUM_ACTUATORS; i++) { for (uint8_t i = 0; i < NUM_ACTUATORS; i++) {
rc_write_angle(AP_MOTORS_MOT_1+i, _actuator_out[i] * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE); rc_write_angle(AP_MOTORS_MOT_1 + i, _actuator_out[i] * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE);
} }
set_actuator_with_slew(_actuator[5], thrust_to_actuator(_thrust_out)); set_actuator_with_slew(_actuator[5], thrust_to_actuator(_thrust_out));
set_actuator_with_slew(_actuator[6], thrust_to_actuator(_thrust_out)); set_actuator_with_slew(_actuator[6], thrust_to_actuator(_thrust_out));
@ -134,9 +133,9 @@ void AP_MotorsSingle::output_armed_stabilizing()
float yaw_thrust; // yaw thrust input value, +/- 1.0 float yaw_thrust; // yaw thrust input value, +/- 1.0
float throttle_thrust; // throttle thrust input value, 0.0 - 1.0 float throttle_thrust; // throttle thrust input value, 0.0 - 1.0
float throttle_avg_max; // throttle thrust average maximum value, 0.0 - 1.0 float throttle_avg_max; // throttle thrust average maximum value, 0.0 - 1.0
float thrust_min_rpy; // the minimum throttle setting that will not limit the roll and pitch output float thrust_min_rpy; // the minimum throttle setting that will not limit the roll and pitch output
float thr_adj; // the difference between the pilot's desired throttle and throttle_thrust_best_rpy float thr_adj; // the difference between the pilot's desired throttle and throttle_thrust_best_rpy
float rp_scale = 1.0f; // this is used to scale the roll, pitch and yaw to fit within the motor limits float rp_scale = 1.0f; // this is used to scale the roll, pitch and yaw to fit within the motor limits
float actuator_allowed = 0.0f; // amount of yaw we can fit in float actuator_allowed = 0.0f; // amount of yaw we can fit in
float actuator[NUM_ACTUATORS]; // combined roll, pitch and yaw thrusts for each actuator float actuator[NUM_ACTUATORS]; // combined roll, pitch and yaw thrusts for each actuator
float actuator_max = 0.0f; // maximum actuator value float actuator_max = 0.0f; // maximum actuator value
@ -167,7 +166,7 @@ void AP_MotorsSingle::output_armed_stabilizing()
if (is_zero(rp_thrust_max)) { if (is_zero(rp_thrust_max)) {
rp_scale = 1.0f; rp_scale = 1.0f;
} 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_pitch = true;
} }
@ -209,10 +208,10 @@ void AP_MotorsSingle::output_armed_stabilizing()
} }
// limit thrust out for calculation of actuator gains // limit thrust out for calculation of actuator gains
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);
// calculate the maximum allowed actuator output and maximum requested actuator output // calculate the maximum allowed actuator output and maximum requested actuator output
for (uint8_t i=0; i<NUM_ACTUATORS; i++) { for (uint8_t i = 0; i < NUM_ACTUATORS; i++) {
if (actuator_max > fabsf(actuator[i])) { if (actuator_max > fabsf(actuator[i])) {
actuator_max = fabsf(actuator[i]); actuator_max = fabsf(actuator[i]);
} }
@ -222,7 +221,7 @@ void AP_MotorsSingle::output_armed_stabilizing()
// 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_pitch = true;
limit.yaw = true; limit.yaw = true;
rp_scale = thrust_out_actuator/actuator_max; rp_scale = thrust_out_actuator / actuator_max;
} else { } else {
rp_scale = 1.0f; rp_scale = 1.0f;
} }
@ -231,8 +230,8 @@ void AP_MotorsSingle::output_armed_stabilizing()
// static thrust is proportional to the airflow velocity squared // static thrust is proportional to the airflow velocity squared
// therefore the torque of the roll and pitch actuators should be approximately proportional to // therefore the torque of the roll and pitch actuators should be approximately proportional to
// the angle of attack multiplied by the static thrust. // the angle of attack multiplied by the static thrust.
for (uint8_t i=0; i<NUM_ACTUATORS; i++) { for (uint8_t i = 0; i < NUM_ACTUATORS; i++) {
_actuator_out[i] = constrain_float(rp_scale*actuator[i]/thrust_out_actuator, -1.0f, 1.0f); _actuator_out[i] = constrain_float(rp_scale * actuator[i] / thrust_out_actuator, -1.0f, 1.0f);
} }
} }

4
libraries/AP_Motors/AP_MotorsTailsitter.cpp
View File

@ -153,8 +153,8 @@ void AP_MotorsTailsitter::output_armed_stabilizing()
} }
// calculate left and right throttle outputs // calculate left and right throttle outputs
_thrust_left = throttle_thrust + roll_thrust*0.5f; _thrust_left = throttle_thrust + roll_thrust * 0.5f;
_thrust_right = throttle_thrust - roll_thrust*0.5f; _thrust_right = throttle_thrust - roll_thrust * 0.5f;
// if max thrust is more than one reduce average throttle // if max thrust is more than one reduce average throttle
thrust_max = MAX(_thrust_right,_thrust_left); thrust_max = MAX(_thrust_right,_thrust_left);

54
libraries/AP_Motors/AP_MotorsTri.cpp
View File

@ -31,7 +31,7 @@ void AP_MotorsTri::init(motor_frame_class frame_class, motor_frame_type frame_ty
add_motor_num(AP_MOTORS_MOT_1); add_motor_num(AP_MOTORS_MOT_1);
add_motor_num(AP_MOTORS_MOT_2); add_motor_num(AP_MOTORS_MOT_2);
add_motor_num(AP_MOTORS_MOT_4); add_motor_num(AP_MOTORS_MOT_4);
// set update rate for the 3 motors (but not the servo on channel 7) // set update rate for the 3 motors (but not the servo on channel 7)
set_update_rate(_speed_hz); set_update_rate(_speed_hz);
@ -47,7 +47,7 @@ void AP_MotorsTri::init(motor_frame_class frame_class, motor_frame_type frame_ty
// don't set initialised_ok // don't set initialised_ok
return; return;
} }
// allow mapping of motor7 // allow mapping of motor7
add_motor_num(AP_MOTORS_CH_TRI_YAW); add_motor_num(AP_MOTORS_CH_TRI_YAW);
@ -62,7 +62,7 @@ void AP_MotorsTri::set_frame_class_and_type(motor_frame_class frame_class, motor
} }
// set update rate to motors - a value in hertz // set update rate to motors - a value in hertz
void AP_MotorsTri::set_update_rate( uint16_t speed_hz ) void AP_MotorsTri::set_update_rate(uint16_t speed_hz)
{ {
// record requested speed // record requested speed
_speed_hz = speed_hz; _speed_hz = speed_hz;
@ -188,13 +188,13 @@ void AP_MotorsTri::output_armed_stabilizing()
// set rpy_low and rpy_high to the lowest and highest values of the motors // set rpy_low and rpy_high to the lowest and highest values of the motors
// record lowest roll pitch command // record lowest roll pitch command
rpy_low = MIN(_thrust_right,_thrust_left); rpy_low = MIN(_thrust_right, _thrust_left);
rpy_high = MAX(_thrust_right,_thrust_left); rpy_high = MAX(_thrust_right, _thrust_left);
if (rpy_low > _thrust_rear){ if (rpy_low > _thrust_rear) {
rpy_low = _thrust_rear; rpy_low = _thrust_rear;
} }
// check to see if the rear motor will reach maximum thrust before the front two motors // check to see if the rear motor will reach maximum thrust before the front two motors
if ((1.0f - rpy_high) > (pivot_thrust_max - _thrust_rear)){ if ((1.0f - rpy_high) > (pivot_thrust_max - _thrust_rear)) {
thrust_max = pivot_thrust_max; thrust_max = pivot_thrust_max;
rpy_high = _thrust_rear; rpy_high = _thrust_rear;
} }
@ -210,41 +210,41 @@ void AP_MotorsTri::output_armed_stabilizing()
// We will choose #2 (a mix of pilot and hover throttle) only when the throttle is quite low. We favor reducing throttle instead of better yaw control because the pilot has commanded it // We will choose #2 (a mix of pilot and hover throttle) only when the throttle is quite low. We favor reducing throttle instead of better yaw control because the pilot has commanded it
// check everything fits // check everything fits
throttle_thrust_best_rpy = MIN(0.5f*thrust_max - (rpy_low+rpy_high)/2.0, throttle_avg_max); throttle_thrust_best_rpy = MIN(0.5f * thrust_max - (rpy_low + rpy_high) / 2.0, throttle_avg_max);
if(is_zero(rpy_low)){ if (is_zero(rpy_low)) {
rpy_scale = 1.0f; rpy_scale = 1.0f;
} else { } else {
rpy_scale = constrain_float(-throttle_thrust_best_rpy/rpy_low, 0.0f, 1.0f); rpy_scale = constrain_float(-throttle_thrust_best_rpy / rpy_low, 0.0f, 1.0f);
} }
// calculate how close the motors can come to the desired throttle // calculate how close the motors can come to the desired throttle
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_pitch = true;
if (thr_adj > 0.0f){ if (thr_adj > 0.0f) {
limit.throttle_upper = true; limit.throttle_upper = true;
} }
thr_adj = 0.0f; thr_adj = 0.0f;
}else{ } else {
if(thr_adj < -(throttle_thrust_best_rpy+rpy_low)){ if (thr_adj < -(throttle_thrust_best_rpy + rpy_low)) {
// Throttle can't be reduced to desired value // Throttle can't be reduced to desired value
thr_adj = -(throttle_thrust_best_rpy+rpy_low); thr_adj = -(throttle_thrust_best_rpy + rpy_low);
}else if(thr_adj > thrust_max - (throttle_thrust_best_rpy+rpy_high)){ } else if (thr_adj > thrust_max - (throttle_thrust_best_rpy + rpy_high)) {
// Throttle can't be increased to desired value // Throttle can't be increased to desired value
thr_adj = thrust_max - (throttle_thrust_best_rpy+rpy_high); thr_adj = thrust_max - (throttle_thrust_best_rpy + rpy_high);
limit.throttle_upper = true; limit.throttle_upper = true;
} }
} }
// add scaled roll, pitch, constrained yaw and throttle for each motor // add scaled roll, pitch, constrained yaw and throttle for each motor
_thrust_right = throttle_thrust_best_rpy+thr_adj + rpy_scale*_thrust_right; _thrust_right = throttle_thrust_best_rpy + thr_adj + rpy_scale * _thrust_right;
_thrust_left = throttle_thrust_best_rpy+thr_adj + rpy_scale*_thrust_left; _thrust_left = throttle_thrust_best_rpy + thr_adj + rpy_scale * _thrust_left;
_thrust_rear = throttle_thrust_best_rpy+thr_adj + rpy_scale*_thrust_rear; _thrust_rear = throttle_thrust_best_rpy + thr_adj + rpy_scale * _thrust_rear;
// scale pivot thrust to account for pivot angle // scale pivot thrust to account for pivot angle
// we should not need to check for divide by zero as _pivot_angle is constrained to the 5deg ~ 80 deg range // we should not need to check for divide by zero as _pivot_angle is constrained to the 5deg ~ 80 deg range
_thrust_rear = _thrust_rear/cosf(_pivot_angle); _thrust_rear = _thrust_rear / cosf(_pivot_angle);
// constrain all outputs to 0.0f to 1.0f // constrain all outputs to 0.0f to 1.0f
// test code should be run with these lines commented out as they should not do anything // test code should be run with these lines commented out as they should not do anything
@ -296,10 +296,10 @@ int16_t AP_MotorsTri::calc_yaw_radio_output(float yaw_input, float yaw_input_max
yaw_input = -yaw_input; yaw_input = -yaw_input;
} }
if (yaw_input >= 0){ if (yaw_input >= 0) {
ret = (_yaw_servo->get_trim() + (yaw_input/yaw_input_max * (_yaw_servo->get_output_max() - _yaw_servo->get_trim()))); ret = (_yaw_servo->get_trim() + (yaw_input / yaw_input_max * (_yaw_servo->get_output_max() - _yaw_servo->get_trim())));
} else { } else {
ret = (_yaw_servo->get_trim() + (yaw_input/yaw_input_max * (_yaw_servo->get_trim() - _yaw_servo->get_output_min()))); ret = (_yaw_servo->get_trim() + (yaw_input / yaw_input_max * (_yaw_servo->get_trim() - _yaw_servo->get_output_min())));
} }
return ret; return ret;
@ -321,8 +321,8 @@ void AP_MotorsTri::thrust_compensation(void)
// extract compensated thrust values // extract compensated thrust values
_thrust_right = thrust[0]; _thrust_right = thrust[0];
_thrust_left = thrust[1]; _thrust_left = thrust[1];
_thrust_rear = thrust[3]; _thrust_rear = thrust[3];
} }
} }
@ -347,7 +347,7 @@ float AP_MotorsTri::get_roll_factor(uint8_t i)
case AP_MOTORS_MOT_1: case AP_MOTORS_MOT_1:
ret = -1.0f; ret = -1.0f;
break; break;
// left motor // left motor
case AP_MOTORS_MOT_2: case AP_MOTORS_MOT_2:
ret = 1.0f; ret = 1.0f;
break; break;

6
libraries/AP_Motors/AP_Motors_Class.cpp
View File

@ -150,8 +150,8 @@ void AP_Motors::rc_set_freq(uint32_t mask, uint16_t freq_hz)
uint32_t AP_Motors::rc_map_mask(uint32_t mask) const uint32_t AP_Motors::rc_map_mask(uint32_t mask) const
{ {
uint32_t mask2 = 0; uint32_t mask2 = 0;
for (uint8_t i=0; i<32; i++) { for (uint8_t i = 0; i < 32; i++) {
uint32_t bit = 1UL<<i; uint32_t bit = 1UL << i;
if (mask & bit) { if (mask & bit) {
SRV_Channel::Aux_servo_function_t function = SRV_Channels::get_motor_function(i); SRV_Channel::Aux_servo_function_t function = SRV_Channels::get_motor_function(i);
mask2 |= SRV_Channels::get_output_channel_mask(function); mask2 |= SRV_Channels::get_output_channel_mask(function);
@ -166,7 +166,7 @@ uint32_t AP_Motors::rc_map_mask(uint32_t mask) const
void AP_Motors::add_motor_num(int8_t motor_num) void AP_Motors::add_motor_num(int8_t motor_num)
{ {
// ensure valid motor number is provided // ensure valid motor number is provided
if( motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS ) { if (motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS) {
uint8_t chan; uint8_t chan;
SRV_Channel::Aux_servo_function_t function = SRV_Channels::get_motor_function(motor_num); SRV_Channel::Aux_servo_function_t function = SRV_Channels::get_motor_function(motor_num);
SRV_Channels::set_aux_channel_default(function, motor_num); SRV_Channels::set_aux_channel_default(function, motor_num);

10
libraries/AP_Motors/AP_Motors_Class.h
View File

@ -65,7 +65,7 @@ public:
AP_Motors(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT); AP_Motors(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT);
// singleton support // singleton support
static AP_Motors *get_singleton(void) { return _singleton; } static AP_Motors *get_singleton(void) { return _singleton; }
// check initialisation succeeded // check initialisation succeeded
bool initialised_ok() const { return _flags.initialised_ok; } bool initialised_ok() const { return _flags.initialised_ok; }
@ -85,7 +85,7 @@ public:
void set_pitch(float pitch_in) { _pitch_in = pitch_in; }; // range -1 ~ +1 void set_pitch(float pitch_in) { _pitch_in = pitch_in; }; // range -1 ~ +1
void set_yaw(float yaw_in) { _yaw_in = yaw_in; }; // range -1 ~ +1 void set_yaw(float yaw_in) { _yaw_in = yaw_in; }; // range -1 ~ +1
void set_throttle(float throttle_in) { _throttle_in = throttle_in; }; // range 0 ~ 1 void set_throttle(float throttle_in) { _throttle_in = throttle_in; }; // range 0 ~ 1
void set_throttle_avg_max(float throttle_avg_max) { _throttle_avg_max = constrain_float(throttle_avg_max,0.0f,1.0f); }; // range 0 ~ 1 void set_throttle_avg_max(float throttle_avg_max) { _throttle_avg_max = constrain_float(throttle_avg_max, 0.0f, 1.0f); }; // range 0 ~ 1
void set_throttle_filter_cutoff(float filt_hz) { _throttle_filter.set_cutoff_frequency(filt_hz); } void set_throttle_filter_cutoff(float filt_hz) { _throttle_filter.set_cutoff_frequency(filt_hz); }
void set_forward(float forward_in) { _forward_in = forward_in; }; // range -1 ~ +1 void set_forward(float forward_in) { _forward_in = forward_in; }; // range -1 ~ +1
void set_lateral(float lateral_in) { _lateral_in = lateral_in; }; // range -1 ~ +1 void set_lateral(float lateral_in) { _lateral_in = lateral_in; }; // range -1 ~ +1
@ -94,8 +94,8 @@ public:
float get_roll() const { return _roll_in; } float get_roll() const { return _roll_in; }
float get_pitch() const { return _pitch_in; } float get_pitch() const { return _pitch_in; }
float get_yaw() const { return _yaw_in; } float get_yaw() const { return _yaw_in; }
float get_throttle() const { return constrain_float(_throttle_filter.get(),0.0f,1.0f); } float get_throttle() const { return constrain_float(_throttle_filter.get(), 0.0f, 1.0f); }
float get_throttle_bidirectional() const { return constrain_float(2*(_throttle_filter.get()-0.5f),-1.0f,1.0f); } float get_throttle_bidirectional() const { return constrain_float(2 * (_throttle_filter.get() - 0.5f), -1.0f, 1.0f); }
float get_forward() const { return _forward_in; } float get_forward() const { return _forward_in; }
float get_lateral() const { return _lateral_in; } float get_lateral() const { return _lateral_in; }
virtual float get_throttle_hover() const = 0; virtual float get_throttle_hover() const = 0;
@ -189,7 +189,7 @@ public:
protected: protected:
// output functions that should be overloaded by child classes // output functions that should be overloaded by child classes
virtual void output_armed_stabilizing()=0; virtual void output_armed_stabilizing() = 0;
virtual void rc_write(uint8_t chan, uint16_t pwm); virtual void rc_write(uint8_t chan, uint16_t pwm);
virtual void rc_write_angle(uint8_t chan, int16_t angle_cd); virtual void rc_write_angle(uint8_t chan, int16_t angle_cd);
virtual void rc_set_freq(uint32_t mask, uint16_t freq_hz); virtual void rc_set_freq(uint32_t mask, uint16_t freq_hz);