Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-28 19:48:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: sport mode restructured to match althold and feedback from mid-stick

Browse Source 
This modifies sport mode to be structured like althold and loiter flight modes so that ongoing maintenance of the modes is hopefully easier.
Also changes throttle feedback to occur from mid-stick
This commit is contained in:
Randy Mackay 2016-08-01 10:49:44 +09:00
parent 53f0216269
commit 88da5bd453
2 changed files with 87 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

117
ArduCopter/control_sport.cpp
View File

@ -24,30 +24,21 @@ bool Copter::sport_init(bool ignore_checks)
// should be called at 100hz or more
void Copter::sport_run()
{
float target_roll_rate, target_pitch_rate, target_yaw_rate;
float target_climb_rate = 0;
SportModeState sport_state;
float takeoff_climb_rate = 0.0f;
// initialize vertical speed and acceleration
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
pos_control.set_accel_z(g.pilot_accel_z);
// if not armed or throttle at zero, set throttle to zero and exit immediately
if (!motors.armed() || ap.throttle_zero || !motors.get_interlock()) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
pos_control.relax_alt_hold_controllers(0.0f);
return;
}
// apply SIMPLE mode transform
update_simple_mode();
// get pilot's desired roll and pitch rates
// calculate rate requests
target_roll_rate = channel_roll->get_control_in() * g.acro_rp_p;
target_pitch_rate = channel_pitch->get_control_in() * g.acro_rp_p;
float target_roll_rate = channel_roll->get_control_in() * g.acro_rp_p;
float target_pitch_rate = channel_pitch->get_control_in() * g.acro_rp_p;
int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor);
target_roll_rate -= constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_roll;
@ -69,41 +60,91 @@ void Copter::sport_run()
}
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
// get pilot desired climb rate
target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max);
// get takeoff adjusted pilot and takeoff climb rates
takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);
#if FRAME_CONFIG == HELI_FRAME
// helicopters are held on the ground until rotor speed runup has finished
bool takeoff_triggered = (ap.land_complete && (target_climb_rate > 0.0f) && motors.rotor_runup_complete());
#else
bool takeoff_triggered = ap.land_complete && (target_climb_rate > 0.0f);
#endif
// check for take-off
if (ap.land_complete && (takeoff_state.running || (channel_throttle->get_control_in() > get_takeoff_trigger_throttle()))) {
if (!takeoff_state.running) {
takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
}
// indicate we are taking off
set_land_complete(false);
// clear i term when we're taking off
set_throttle_takeoff();
// State Machine Determination
if (!motors.armed() || !motors.get_interlock()) {
sport_state = Sport_MotorStopped;
} else if (takeoff_state.running || takeoff_triggered) {
sport_state = Sport_Takeoff;
} else if (!ap.auto_armed || ap.land_complete) {
sport_state = Sport_Landed;
} else {
sport_state = Sport_Flying;
}
// reset target lean angles and heading while landed
if (ap.land_complete) {
if (ap.throttle_zero) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
}else{
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
}
// move throttle to between minimum and non-takeoff-throttle to keep us on the ground
attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->get_control_in()),false,g.throttle_filt);
pos_control.relax_alt_hold_controllers(0.0f);
}else{
// State Machine
switch (sport_state) {
case Sport_MotorStopped:
motors.set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
attitude_control.input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);
#if FRAME_CONFIG == HELI_FRAME
// force descent rate and call position controller
pos_control.set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
#else
attitude_control.relax_attitude_controllers();
attitude_control.reset_rate_controller_I_terms();
attitude_control.set_yaw_target_to_current_heading();
pos_control.relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
#endif
pos_control.update_z_controller();
break;
case Sport_Takeoff:
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// initiate take-off
if (!takeoff_state.running) {
takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
// indicate we are taking off
set_land_complete(false);
// clear i terms
set_throttle_takeoff();
}
// get take-off adjusted pilot and takeoff climb rates
takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);
// call attitude controller
attitude_control.input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);
// call position controller
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
pos_control.update_z_controller();
break;
case Sport_Landed:
// set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle)
if (target_climb_rate < 0.0f) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
} else {
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
}
attitude_control.reset_rate_controller_I_terms();
attitude_control.set_yaw_target_to_current_heading();
attitude_control.input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);
pos_control.relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
pos_control.update_z_controller();
break;
case Sport_Flying:
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// call attitude controller
attitude_control.input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);
@ -115,7 +156,7 @@ void Copter::sport_run()
// call position controller
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
pos_control.update_z_controller();
break;
}
}

8
ArduCopter/defines.h
View File

@ -238,6 +238,14 @@ enum LoiterModeState {
Loiter_Landed
};
// Sport states
enum SportModeState {
Sport_MotorStopped,
Sport_Takeoff,
Sport_Flying,
Sport_Landed
};
// Flip states
enum FlipState {
Flip_Start,