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;
// init
void AP_MotorsCoax::init(motor_frame_class frame_class, motor_frame_type frame_type)
{
// make sure 6 output channels are mapped
for (uint8_t i=0; i<6; i++) {
add_motor_num(CH_1+i);
for (uint8_t i = 0; i < 6; i++) {
add_motor_num(CH_1 + i);
}
// 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;
// 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);
}
// record successful initialisation if what we setup was the desired frame_class
_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
void AP_MotorsCoax::set_update_rate( uint16_t speed_hz )
void AP_MotorsCoax::set_update_rate(uint16_t speed_hz)
{
// record requested speed
_speed_hz = speed_hz;
@ -80,8 +79,8 @@ void AP_MotorsCoax::output_to_motors()
break;
case SpoolState::GROUND_IDLE:
// sends output to motors when armed but not flying
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);
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);
}
set_actuator_with_slew(_actuator[5], 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::SPOOLING_DOWN:
// set motor output based on thrust requests
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);
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);
}
set_actuator_with_slew(_actuator[5], thrust_to_actuator(_thrust_yt_ccw));
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)) {
rp_scale = 1.0f;
} 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) {
limit.roll_pitch = true;
}
@ -196,7 +195,7 @@ void AP_MotorsCoax::output_armed_stabilizing()
_thrust_yt_cw = thrust_out - 0.5f * yaw_thrust;
// 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)) {
limit.roll_pitch = true;
@ -205,8 +204,8 @@ void AP_MotorsCoax::output_armed_stabilizing()
// static thrust is proportional to the airflow velocity squared
// therefore the torque of the roll and pitch actuators should be approximately proportional to
// the angle of attack multiplied by the static thrust.
_actuator_out[0] = roll_thrust/thrust_out_actuator;
_actuator_out[1] = pitch_thrust/thrust_out_actuator;
_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;
_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
void AP_MotorsMatrix::set_update_rate( uint16_t speed_hz )
void AP_MotorsMatrix::set_update_rate(uint16_t speed_hz)
{
// record requested speed
_speed_hz = speed_hz;
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]) {
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)
@ -76,7 +76,7 @@ void AP_MotorsMatrix::output_to_motors()
switch (_spool_state) {
case SpoolState::SHUT_DOWN: {
// 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]) {
_actuator[i] = 0.0f;
}
@ -85,7 +85,7 @@ void AP_MotorsMatrix::output_to_motors()
}
case SpoolState::GROUND_IDLE:
// 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]) {
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::SPOOLING_DOWN:
// 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]) {
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
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[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)
// 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 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]) {
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
float rp_low = 1.0f; // lowest 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]) {
// calculate the thrust outputs for roll and pitch
_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) {
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)) {
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
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) {
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
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.
limit.roll_pitch = true;
}
@ -228,8 +227,8 @@ void AP_MotorsMatrix::output_armed_stabilizing()
// calculate the maximum yaw control that can be used
// todo: make _yaw_headroom 0 to 1
yaw_allowed = (float)_yaw_headroom / 1000.0f;
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 = _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);
if (fabsf(yaw_thrust) > yaw_allowed) {
// not all commanded yaw can be used
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
float rpy_low = 1.0f; // lowest 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]) {
_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) {
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)) {
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
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]) {
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
if (rpy_high-rpy_low > 1.0f) {
rpy_scale = 1.0f / (rpy_high-rpy_low);
if (rpy_high - rpy_low > 1.0f) {
rpy_scale = 1.0f / (rpy_high - rpy_low);
}
if (is_negative(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
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[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
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) {
// turn on this motor
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?
if (output_channel > AP_MOTORS_MAX_NUM_MOTORS -1) {
if (output_channel > AP_MOTORS_MAX_NUM_MOTORS - 1) {
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)
{
// 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
if( !motor_enabled[motor_num] ) {
if (!motor_enabled[motor_num]) {
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)
{
// 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
motor_enabled[motor_num] = false;
_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)
{
// 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);
}
@ -561,10 +560,10 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
break;
case MOTOR_FRAME_TYPE_PLUSREV:
// 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_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_3, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
add_motor(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
break;
default:
// 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
success = false;
break;
} // switch frame_class
} // switch frame_class
// normalise factors to magnitude 0.5
normalise_rpy_factors();
@ -804,7 +803,7 @@ void AP_MotorsMatrix::normalise_rpy_factors()
float yaw_fac = 0.0f;
// 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 (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
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 (!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)) {
_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)) {
_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
AP_MotorsMulticopter::AP_MotorsMulticopter(uint16_t loop_rate, uint16_t speed_hz) :
AP_Motors(loop_rate, speed_hz),
_lift_max(1.0f),
_throttle_limit(1.0f)
AP_Motors(loop_rate, speed_hz),
_lift_max(1.0f),
_throttle_limit(1.0f)
{
AP_Param::setup_object_defaults(this, var_info);
@ -240,7 +240,7 @@ void AP_MotorsMulticopter::output()
// apply any thrust compensation for the frame
thrust_compensation();
// convert rpy_thrust values to pwm
output_to_motors();
@ -253,10 +253,9 @@ void AP_MotorsMulticopter::output_boost_throttle(void)
{
if (_boost_scale > 0) {
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
void AP_MotorsMulticopter::output_min()
@ -270,7 +269,7 @@ void AP_MotorsMulticopter::output_min()
void AP_MotorsMulticopter::update_throttle_filter()
{
if (armed()) {
_throttle_filter.apply(_throttle_in, 1.0f/_loop_rate);
_throttle_filter.apply(_throttle_in, 1.0f / _loop_rate);
// constrain filtered throttle
if (_throttle_filter.get() < 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);
// 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;
_throttle_limit += (loop_interval/(loop_interval+_batt_current_time_constant))*(1.0f - batt_current_ratio);
float loop_interval = 1.0f / _loop_rate;
_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 = constrain_float(_throttle_limit, 0.2f, 1.0f);
// 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
@ -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
return thrust;
}
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());
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());
} 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);
@ -344,7 +343,7 @@ void AP_MotorsMulticopter::update_lift_max_from_batt_voltage()
// sanity check battery_voltage_min is not too small
// if disabled or misconfigured exit immediately
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);
_lift_max = 1.0f;
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);
// 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
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
@ -375,7 +374,7 @@ float AP_MotorsMulticopter::get_compensation_gain() const
#if AP_MOTORS_DENSITY_COMP == 1
// air density ratio is increasing in density / decreasing in altitude
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
return ret;
@ -394,7 +393,7 @@ int16_t AP_MotorsMulticopter::output_to_pwm(float actuator)
}
} else {
// 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;
@ -404,7 +403,7 @@ int16_t AP_MotorsMulticopter::output_to_pwm(float actuator)
float AP_MotorsMulticopter::thrust_to_actuator(float thrust_in)
{
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
@ -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 (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);
}
// 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)) {
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);
}
@ -489,7 +488,7 @@ void AP_MotorsMulticopter::update_throttle_hover(float dt)
{
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.
_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) {
case SpoolState::SHUT_DOWN:
// Motors should be stationary.
// Servos set to their trim values or in a test condition.
case SpoolState::SHUT_DOWN:
// Motors should be stationary.
// Servos set to their trim values or in a test condition.
// set limits flags
limit.roll_pitch = true;
limit.yaw = true;
limit.throttle_lower = true;
limit.throttle_upper = true;
// set limits flags
limit.roll_pitch = true;
limit.yaw = true;
limit.throttle_lower = true;
limit.throttle_upper = true;
// make sure the motors are spooling in the correct direction
if (_spool_desired != DesiredSpoolState::SHUT_DOWN) {
_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;
// make sure the motors are spooling in the correct direction
if (_spool_desired != DesiredSpoolState::SHUT_DOWN) {
_spool_state = SpoolState::GROUND_IDLE;
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;
// set and increment ramp variables
_spin_up_ratio = 0.0f;
_throttle_thrust_max = 0.0f;
// make sure the motors are spooling in the correct direction
if (_spool_desired != DesiredSpoolState::THROTTLE_UNLIMITED ){
_spool_state = SpoolState::SPOOLING_DOWN;
break;
}
// initialise motor failure variables
_thrust_boost = false;
_thrust_boost_ratio = 0.0f;
break;
// set and increment ramp variables
_spin_up_ratio = 1.0f;
_throttle_thrust_max += 1.0f/(_spool_up_time*_loop_rate);
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 (_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));
if (_spin_up_ratio <= 0.0f) {
_spin_up_ratio = 0.0f;
_spool_state = SpoolState::SHUT_DOWN;
}
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) {
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;
}
// 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;
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()) {
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
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]) {
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
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 (mask & (1U<<i)) {
if (mask & (1U << i)) {
/*
apply rudder mixing differential thrust
copter frame roll is plane frame yaw as this only
apples to either tilted motors or tailsitters
*/
apply rudder mixing differential thrust
copter frame roll is plane frame yaw as this only
apples to either tilted motors or tailsitters
*/
float diff_thrust = get_roll_factor(i) * rudder_dt * 0.5f;
set_actuator_with_slew(_actuator[i], thrust_to_actuator(thrust + diff_thrust));
int16_t pwm_output = get_pwm_output_min() +
(get_pwm_output_max()-get_pwm_output_min()) * _actuator[i];
int16_t pwm_output = get_pwm_output_min() + (get_pwm_output_max() - get_pwm_output_min()) * _actuator[i];
rc_write(i, pwm_output);
} else {
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;
// init
void AP_MotorsSingle::init(motor_frame_class frame_class, motor_frame_type frame_type)
{
// make sure 6 output channels are mapped
for (uint8_t i=0; i<6; i++) {
add_motor_num(CH_1+i);
for (uint8_t i = 0; i < 6; i++) {
add_motor_num(CH_1 + i);
}
// 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;
// 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);
}
@ -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
void AP_MotorsSingle::set_update_rate( uint16_t speed_hz )
void AP_MotorsSingle::set_update_rate(uint16_t speed_hz)
{
// record requested speed
_speed_hz = speed_hz;
@ -83,8 +82,8 @@ void AP_MotorsSingle::output_to_motors()
break;
case SpoolState::GROUND_IDLE:
// sends output to motors when armed but not flying
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);
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);
}
set_actuator_with_slew(_actuator[5], 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::SPOOLING_DOWN:
// set motor output based on thrust requests
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);
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);
}
set_actuator_with_slew(_actuator[5], 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 throttle_thrust; // throttle thrust input 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 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[NUM_ACTUATORS]; // combined roll, pitch and yaw thrusts for each actuator
float actuator_max = 0.0f; // maximum actuator value
@ -167,7 +166,7 @@ void AP_MotorsSingle::output_armed_stabilizing()
if (is_zero(rp_thrust_max)) {
rp_scale = 1.0f;
} 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) {
limit.roll_pitch = true;
}
@ -209,10 +208,10 @@ void AP_MotorsSingle::output_armed_stabilizing()
}
// 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
for (uint8_t i=0; i<NUM_ACTUATORS; i++) {
for (uint8_t i = 0; i < NUM_ACTUATORS; i++) {
if (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
limit.roll_pitch = true;
limit.yaw = true;
rp_scale = thrust_out_actuator/actuator_max;
rp_scale = thrust_out_actuator / actuator_max;
} else {
rp_scale = 1.0f;
}
@ -231,8 +230,8 @@ void AP_MotorsSingle::output_armed_stabilizing()
// static thrust is proportional to the airflow velocity squared
// therefore the torque of the roll and pitch actuators should be approximately proportional to
// the angle of attack multiplied by the static thrust.
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);
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);
}
}

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
_thrust_left = throttle_thrust + roll_thrust*0.5f;
_thrust_right = throttle_thrust - roll_thrust*0.5f;
_thrust_left = throttle_thrust + roll_thrust * 0.5f;
_thrust_right = throttle_thrust - roll_thrust * 0.5f;
// if max thrust is more than one reduce average throttle
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_2);
add_motor_num(AP_MOTORS_MOT_4);
// set update rate for the 3 motors (but not the servo on channel 7)
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
return;
}
// allow mapping of motor7
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
void AP_MotorsTri::set_update_rate( uint16_t speed_hz )
void AP_MotorsTri::set_update_rate(uint16_t speed_hz)
{
// record requested speed
_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
// record lowest roll pitch command
rpy_low = MIN(_thrust_right,_thrust_left);
rpy_high = MAX(_thrust_right,_thrust_left);
if (rpy_low > _thrust_rear){
rpy_low = MIN(_thrust_right, _thrust_left);
rpy_high = MAX(_thrust_right, _thrust_left);
if (rpy_low > _thrust_rear) {
rpy_low = _thrust_rear;
}
// 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;
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
// check everything fits
throttle_thrust_best_rpy = MIN(0.5f*thrust_max - (rpy_low+rpy_high)/2.0, throttle_avg_max);
if(is_zero(rpy_low)){
throttle_thrust_best_rpy = MIN(0.5f * thrust_max - (rpy_low + rpy_high) / 2.0, throttle_avg_max);
if (is_zero(rpy_low)) {
rpy_scale = 1.0f;
} 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
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.
limit.roll_pitch = true;
if (thr_adj > 0.0f){
if (thr_adj > 0.0f) {
limit.throttle_upper = true;
}
thr_adj = 0.0f;
}else{
if(thr_adj < -(throttle_thrust_best_rpy+rpy_low)){
} else {
if (thr_adj < -(throttle_thrust_best_rpy + rpy_low)) {
// Throttle can't be reduced to desired value
thr_adj = -(throttle_thrust_best_rpy+rpy_low);
}else if(thr_adj > thrust_max - (throttle_thrust_best_rpy+rpy_high)){
thr_adj = -(throttle_thrust_best_rpy + rpy_low);
} else if (thr_adj > thrust_max - (throttle_thrust_best_rpy + rpy_high)) {
// 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;
}
}
// add scaled roll, pitch, constrained yaw and throttle for each motor
_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_rear = throttle_thrust_best_rpy+thr_adj + rpy_scale*_thrust_rear;
_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_rear = throttle_thrust_best_rpy + thr_adj + rpy_scale * _thrust_rear;
// 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
_thrust_rear = _thrust_rear/cosf(_pivot_angle);
_thrust_rear = _thrust_rear / cosf(_pivot_angle);
// 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
@ -296,10 +296,10 @@ int16_t AP_MotorsTri::calc_yaw_radio_output(float yaw_input, float yaw_input_max
yaw_input = -yaw_input;
}
if (yaw_input >= 0){
ret = (_yaw_servo->get_trim() + (yaw_input/yaw_input_max * (_yaw_servo->get_output_max() - _yaw_servo->get_trim())));
if (yaw_input >= 0) {
ret = (_yaw_servo->get_trim() + (yaw_input / yaw_input_max * (_yaw_servo->get_output_max() - _yaw_servo->get_trim())));
} 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;
@ -321,8 +321,8 @@ void AP_MotorsTri::thrust_compensation(void)
// extract compensated thrust values
_thrust_right = thrust[0];
_thrust_left = thrust[1];
_thrust_rear = thrust[3];
_thrust_left = thrust[1];
_thrust_rear = thrust[3];
}
}
@ -347,7 +347,7 @@ float AP_MotorsTri::get_roll_factor(uint8_t i)
case AP_MOTORS_MOT_1:
ret = -1.0f;
break;
// left motor
// left motor
case AP_MOTORS_MOT_2:
ret = 1.0f;
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 mask2 = 0;
for (uint8_t i=0; i<32; i++) {
uint32_t bit = 1UL<<i;
for (uint8_t i = 0; i < 32; i++) {
uint32_t bit = 1UL << i;
if (mask & bit) {
SRV_Channel::Aux_servo_function_t function = SRV_Channels::get_motor_function(i);
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)
{
// 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;
SRV_Channel::Aux_servo_function_t function = SRV_Channels::get_motor_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);
// singleton support
static AP_Motors *get_singleton(void) { return _singleton; }
static AP_Motors *get_singleton(void) { return _singleton; }
// check initialisation succeeded
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_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_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_forward(float forward_in) { _forward_in = forward_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_pitch() const { return _pitch_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_bidirectional() const { return constrain_float(2*(_throttle_filter.get()-0.5f),-1.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_forward() const { return _forward_in; }
float get_lateral() const { return _lateral_in; }
virtual float get_throttle_hover() const = 0;
@ -189,7 +189,7 @@ public:
protected:
// 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_angle(uint8_t chan, int16_t angle_cd);
virtual void rc_set_freq(uint32_t mask, uint16_t freq_hz);