zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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

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
mission-4.1.18
Randy Mackay 9 years ago
parent
53f0216269
commit
88da5bd453
2 changed files with 83 additions and 34 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 109
      ArduCopter/control_sport.cpp
  2. 8
      ArduCopter/defines.h

109
ArduCopter/control_sport.cpp
Unescape View File

@ -24,30 +24,21 @@ bool Copter::sport_init(bool ignore_checks) @@ -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() @@ -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
// 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;
}
// 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);
// check for take-off
if (ap.land_complete && (takeoff_state.running || (channel_throttle->get_control_in() > get_takeoff_trigger_throttle()))) {
// 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();
}
// indicate we are taking off
set_land_complete(false);
// clear i term when we're taking off
set_throttle_takeoff();
}
// get take-off adjusted pilot and takeoff climb rates
takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);
// reset target lean angles and heading while landed
if (ap.land_complete) {
if (ap.throttle_zero) {
// 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{
} 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{
// set motors to full range
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() @@ -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
Unescape View File

@ -238,6 +238,14 @@ enum LoiterModeState { @@ -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,

Write Preview
Loading…
Cancel
Save