2015-05-29 23:12:49 -03:00
|
|
|
#include "Copter.h"
|
|
|
|
|
2017-12-10 23:57:04 -04:00
|
|
|
#include "mode.h"
|
2014-01-24 23:00:07 -04:00
|
|
|
|
2018-03-14 17:14:49 -03:00
|
|
|
#if MODE_ACRO_ENABLED == ENABLED
|
|
|
|
|
2014-01-24 23:00:07 -04:00
|
|
|
/*
|
2016-07-25 15:45:29 -03:00
|
|
|
* Init and run calls for acro flight mode
|
2014-01-24 23:00:07 -04:00
|
|
|
*/
|
|
|
|
|
2019-05-09 23:18:49 -03:00
|
|
|
void ModeAcro::run()
|
2014-01-24 23:00:07 -04:00
|
|
|
{
|
2019-02-28 05:03:23 -04:00
|
|
|
// convert the input to the desired body frame rate
|
2014-02-09 21:26:49 -04:00
|
|
|
float target_roll, target_pitch, target_yaw;
|
2021-09-17 02:54:19 -03:00
|
|
|
get_pilot_desired_angle_rates(channel_roll->norm_input_dz(), channel_pitch->norm_input_dz(), channel_yaw->norm_input_dz(), target_roll, target_pitch, target_yaw);
|
2014-01-24 23:00:07 -04:00
|
|
|
|
2019-02-28 05:03:23 -04:00
|
|
|
if (!motors->armed()) {
|
|
|
|
// Motors should be Stopped
|
2019-04-09 09:16:58 -03:00
|
|
|
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
|
2021-09-06 12:33:40 -03:00
|
|
|
} else if (copter.ap.throttle_zero && copter.air_mode != AirMode::AIRMODE_ENABLED) {
|
|
|
|
// Attempting to Land, if airmode is enabled only an actual landing will spool down the motors
|
2019-04-09 09:16:58 -03:00
|
|
|
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
|
2019-02-28 05:03:23 -04:00
|
|
|
} else {
|
2019-04-09 09:16:58 -03:00
|
|
|
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
|
2014-01-30 08:44:44 -04:00
|
|
|
}
|
|
|
|
|
2019-04-09 09:16:58 -03:00
|
|
|
switch (motors->get_spool_state()) {
|
|
|
|
case AP_Motors::SpoolState::SHUT_DOWN:
|
2019-02-28 05:03:23 -04:00
|
|
|
// Motors Stopped
|
2021-07-09 09:12:25 -03:00
|
|
|
attitude_control->reset_target_and_rate(true);
|
2019-02-28 05:03:23 -04:00
|
|
|
attitude_control->reset_rate_controller_I_terms();
|
2019-04-09 09:16:58 -03:00
|
|
|
break;
|
2019-11-03 22:54:47 -04:00
|
|
|
|
2019-04-09 09:16:58 -03:00
|
|
|
case AP_Motors::SpoolState::GROUND_IDLE:
|
2019-02-28 05:03:23 -04:00
|
|
|
// Landed
|
2021-05-24 20:42:49 -03:00
|
|
|
attitude_control->reset_target_and_rate();
|
2020-12-20 17:40:58 -04:00
|
|
|
attitude_control->reset_rate_controller_I_terms_smoothly();
|
2019-04-09 09:16:58 -03:00
|
|
|
break;
|
2019-11-03 22:54:47 -04:00
|
|
|
|
2019-04-09 09:16:58 -03:00
|
|
|
case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
|
2019-02-28 05:03:23 -04:00
|
|
|
// clear landing flag above zero throttle
|
|
|
|
if (!motors->limit.throttle_lower) {
|
|
|
|
set_land_complete(false);
|
|
|
|
}
|
2019-04-09 09:16:58 -03:00
|
|
|
break;
|
2019-11-03 22:54:47 -04:00
|
|
|
|
2019-04-09 09:16:58 -03:00
|
|
|
case AP_Motors::SpoolState::SPOOLING_UP:
|
|
|
|
case AP_Motors::SpoolState::SPOOLING_DOWN:
|
|
|
|
// do nothing
|
|
|
|
break;
|
2019-02-28 05:03:23 -04:00
|
|
|
}
|
2014-01-24 23:00:07 -04:00
|
|
|
|
2014-01-30 20:55:00 -04:00
|
|
|
// run attitude controller
|
2020-06-16 17:01:19 -03:00
|
|
|
if (g2.acro_options.get() & uint8_t(AcroOptions::RATE_LOOP_ONLY)) {
|
|
|
|
attitude_control->input_rate_bf_roll_pitch_yaw_2(target_roll, target_pitch, target_yaw);
|
|
|
|
} else {
|
|
|
|
attitude_control->input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
|
|
|
|
}
|
2014-01-30 20:55:00 -04:00
|
|
|
|
|
|
|
// output pilot's throttle without angle boost
|
2019-03-06 02:36:32 -04:00
|
|
|
attitude_control->set_throttle_out(get_pilot_desired_throttle(),
|
|
|
|
false,
|
|
|
|
copter.g.throttle_filt);
|
2014-01-24 23:00:07 -04:00
|
|
|
}
|
|
|
|
|
2020-06-16 17:01:19 -03:00
|
|
|
bool ModeAcro::init(bool ignore_checks)
|
|
|
|
{
|
|
|
|
if (g2.acro_options.get() & uint8_t(AcroOptions::AIR_MODE)) {
|
|
|
|
disable_air_mode_reset = false;
|
|
|
|
copter.air_mode = AirMode::AIRMODE_ENABLED;
|
|
|
|
}
|
|
|
|
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
void ModeAcro::exit()
|
|
|
|
{
|
|
|
|
if (!disable_air_mode_reset && (g2.acro_options.get() & uint8_t(AcroOptions::AIR_MODE))) {
|
|
|
|
copter.air_mode = AirMode::AIRMODE_DISABLED;
|
|
|
|
}
|
|
|
|
disable_air_mode_reset = false;
|
|
|
|
}
|
|
|
|
|
|
|
|
void ModeAcro::air_mode_aux_changed()
|
|
|
|
{
|
|
|
|
disable_air_mode_reset = true;
|
|
|
|
}
|
|
|
|
|
2019-09-07 20:33:56 -03:00
|
|
|
float ModeAcro::throttle_hover() const
|
|
|
|
{
|
|
|
|
if (g2.acro_thr_mid > 0) {
|
|
|
|
return g2.acro_thr_mid;
|
|
|
|
}
|
|
|
|
return Mode::throttle_hover();
|
|
|
|
}
|
2014-01-24 23:00:07 -04:00
|
|
|
|
2021-09-17 02:54:19 -03:00
|
|
|
// get_pilot_desired_angle_rates - transform pilot's normalised roll pitch and yaw input into a desired lean angle rates
|
|
|
|
// inputs are -1 to 1 and the function returns desired angle rates in centi-degrees-per-second
|
|
|
|
void ModeAcro::get_pilot_desired_angle_rates(float roll_in, float pitch_in, float yaw_in, float &roll_out, float &pitch_out, float &yaw_out)
|
2014-01-24 23:00:07 -04:00
|
|
|
{
|
2014-01-27 00:55:02 -04:00
|
|
|
float rate_limit;
|
2021-09-17 02:54:19 -03:00
|
|
|
Vector3f rate_ef_level_cd, rate_bf_level_cd, rate_bf_request_cd;
|
2014-01-24 23:00:07 -04:00
|
|
|
|
2014-11-10 20:07:08 -04:00
|
|
|
// apply circular limit to pitch and roll inputs
|
2016-04-16 06:58:46 -03:00
|
|
|
float total_in = norm(pitch_in, roll_in);
|
2014-11-10 20:07:08 -04:00
|
|
|
|
2021-09-17 02:54:19 -03:00
|
|
|
if (total_in > 1.0) {
|
|
|
|
float ratio = 1.0 / total_in;
|
2014-11-10 20:07:08 -04:00
|
|
|
roll_in *= ratio;
|
|
|
|
pitch_in *= ratio;
|
|
|
|
}
|
2020-04-05 03:42:32 -03:00
|
|
|
|
2014-08-12 23:25:59 -03:00
|
|
|
// calculate roll, pitch rate requests
|
2021-09-17 02:54:19 -03:00
|
|
|
|
|
|
|
// roll expo
|
2022-06-15 00:08:53 -03:00
|
|
|
rate_bf_request_cd.x = g2.command_model_acro_rp.get_rate() * 100.0 * input_expo(roll_in, g2.command_model_acro_rp.get_expo());
|
2021-09-17 02:54:19 -03:00
|
|
|
|
|
|
|
// pitch expo
|
2022-06-15 00:08:53 -03:00
|
|
|
rate_bf_request_cd.y = g2.command_model_acro_rp.get_rate() * 100.0 * input_expo(pitch_in, g2.command_model_acro_rp.get_expo());
|
2014-08-12 23:25:59 -03:00
|
|
|
|
2021-09-17 02:54:19 -03:00
|
|
|
// yaw expo
|
2022-06-15 00:08:53 -03:00
|
|
|
rate_bf_request_cd.z = g2.command_model_acro_y.get_rate() * 100.0 * input_expo(yaw_in, g2.command_model_acro_y.get_expo());
|
2014-01-24 23:00:07 -04:00
|
|
|
|
|
|
|
// calculate earth frame rate corrections to pull the copter back to level while in ACRO mode
|
|
|
|
|
2019-10-25 03:06:05 -03:00
|
|
|
if (g.acro_trainer != (uint8_t)Trainer::OFF) {
|
2017-05-23 02:02:33 -03:00
|
|
|
|
|
|
|
// get attitude targets
|
|
|
|
const Vector3f att_target = attitude_control->get_att_target_euler_cd();
|
|
|
|
|
2014-02-09 21:26:49 -04:00
|
|
|
// Calculate trainer mode earth frame rate command for roll
|
2017-05-23 02:02:33 -03:00
|
|
|
int32_t roll_angle = wrap_180_cd(att_target.x);
|
2021-09-17 02:54:19 -03:00
|
|
|
rate_ef_level_cd.x = -constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_roll;
|
2014-02-09 21:26:49 -04:00
|
|
|
|
|
|
|
// Calculate trainer mode earth frame rate command for pitch
|
2017-05-23 02:02:33 -03:00
|
|
|
int32_t pitch_angle = wrap_180_cd(att_target.y);
|
2021-09-17 02:54:19 -03:00
|
|
|
rate_ef_level_cd.y = -constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch;
|
2014-02-09 21:26:49 -04:00
|
|
|
|
|
|
|
// Calculate trainer mode earth frame rate command for yaw
|
2021-09-17 02:54:19 -03:00
|
|
|
rate_ef_level_cd.z = 0;
|
2014-02-09 21:26:49 -04:00
|
|
|
|
|
|
|
// Calculate angle limiting earth frame rate commands
|
2019-10-25 03:06:05 -03:00
|
|
|
if (g.acro_trainer == (uint8_t)Trainer::LIMITED) {
|
2018-10-03 00:14:44 -03:00
|
|
|
const float angle_max = copter.aparm.angle_max;
|
|
|
|
if (roll_angle > angle_max){
|
2022-06-15 00:08:53 -03:00
|
|
|
rate_ef_level_cd.x += sqrt_controller(angle_max - roll_angle, g2.command_model_acro_rp.get_rate() * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_roll_max_cdss(), G_Dt);
|
2018-10-03 00:14:44 -03:00
|
|
|
}else if (roll_angle < -angle_max) {
|
2022-06-15 00:08:53 -03:00
|
|
|
rate_ef_level_cd.x += sqrt_controller(-angle_max - roll_angle, g2.command_model_acro_rp.get_rate() * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_roll_max_cdss(), G_Dt);
|
2014-02-09 21:26:49 -04:00
|
|
|
}
|
|
|
|
|
2018-10-03 00:14:44 -03:00
|
|
|
if (pitch_angle > angle_max){
|
2022-06-15 00:08:53 -03:00
|
|
|
rate_ef_level_cd.y += sqrt_controller(angle_max - pitch_angle, g2.command_model_acro_rp.get_rate() * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_pitch_max_cdss(), G_Dt);
|
2018-10-03 00:14:44 -03:00
|
|
|
}else if (pitch_angle < -angle_max) {
|
2022-06-15 00:08:53 -03:00
|
|
|
rate_ef_level_cd.y += sqrt_controller(-angle_max - pitch_angle, g2.command_model_acro_rp.get_rate() * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_pitch_max_cdss(), G_Dt);
|
2014-02-09 21:26:49 -04:00
|
|
|
}
|
2014-01-24 23:00:07 -04:00
|
|
|
}
|
|
|
|
|
2014-02-09 21:26:49 -04:00
|
|
|
// convert earth-frame level rates to body-frame level rates
|
2021-09-17 02:54:19 -03:00
|
|
|
attitude_control->euler_rate_to_ang_vel(attitude_control->get_att_target_euler_cd() * radians(0.01f), rate_ef_level_cd, rate_bf_level_cd);
|
2014-02-09 21:26:49 -04:00
|
|
|
|
|
|
|
// combine earth frame rate corrections with rate requests
|
2019-10-25 03:06:05 -03:00
|
|
|
if (g.acro_trainer == (uint8_t)Trainer::LIMITED) {
|
2021-09-17 02:54:19 -03:00
|
|
|
rate_bf_request_cd.x += rate_bf_level_cd.x;
|
|
|
|
rate_bf_request_cd.y += rate_bf_level_cd.y;
|
|
|
|
rate_bf_request_cd.z += rate_bf_level_cd.z;
|
2014-02-09 21:26:49 -04:00
|
|
|
}else{
|
2019-03-11 13:15:25 -03:00
|
|
|
float acro_level_mix = constrain_float(1-float(MAX(MAX(abs(roll_in), abs(pitch_in)), abs(yaw_in))/4500.0), 0, 1)*ahrs.cos_pitch();
|
2014-02-09 21:26:49 -04:00
|
|
|
|
2021-09-17 02:54:19 -03:00
|
|
|
// Scale levelling rates by stick input
|
|
|
|
rate_bf_level_cd = rate_bf_level_cd * acro_level_mix;
|
2014-02-09 21:26:49 -04:00
|
|
|
|
|
|
|
// Calculate rate limit to prevent change of rate through inverted
|
2021-09-17 02:54:19 -03:00
|
|
|
rate_limit = fabsf(fabsf(rate_bf_request_cd.x)-fabsf(rate_bf_level_cd.x));
|
|
|
|
rate_bf_request_cd.x += rate_bf_level_cd.x;
|
|
|
|
rate_bf_request_cd.x = constrain_float(rate_bf_request_cd.x, -rate_limit, rate_limit);
|
2014-02-09 21:26:49 -04:00
|
|
|
|
|
|
|
// Calculate rate limit to prevent change of rate through inverted
|
2021-09-17 02:54:19 -03:00
|
|
|
rate_limit = fabsf(fabsf(rate_bf_request_cd.y)-fabsf(rate_bf_level_cd.y));
|
|
|
|
rate_bf_request_cd.y += rate_bf_level_cd.y;
|
|
|
|
rate_bf_request_cd.y = constrain_float(rate_bf_request_cd.y, -rate_limit, rate_limit);
|
2014-02-09 21:26:49 -04:00
|
|
|
|
|
|
|
// Calculate rate limit to prevent change of rate through inverted
|
2021-09-17 02:54:19 -03:00
|
|
|
rate_limit = fabsf(fabsf(rate_bf_request_cd.z)-fabsf(rate_bf_level_cd.z));
|
|
|
|
rate_bf_request_cd.z += rate_bf_level_cd.z;
|
|
|
|
rate_bf_request_cd.z = constrain_float(rate_bf_request_cd.z, -rate_limit, rate_limit);
|
2014-02-09 21:26:49 -04:00
|
|
|
}
|
2014-01-24 23:00:07 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// hand back rate request
|
2021-09-17 02:54:19 -03:00
|
|
|
roll_out = rate_bf_request_cd.x;
|
|
|
|
pitch_out = rate_bf_request_cd.y;
|
|
|
|
yaw_out = rate_bf_request_cd.z;
|
2014-01-24 23:00:07 -04:00
|
|
|
}
|
2018-03-14 17:14:49 -03:00
|
|
|
#endif
|