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

Copter: guided sets desired spool state

This commit is contained in:
Leonard Hall 2016-01-13 16:10:45 +09:00 committed by Randy Mackay
parent f221d4d757
commit 42f5de44d4
1 changed files with 35 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
ArduCopter/control_guided.cpp
View File

@ -265,12 +265,14 @@ void Copter::guided_run()
void Copter::guided_takeoff_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!ap.auto_armed || !motors.get_interlock()) {
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
#else
motors.set_desired_spool_state(AP_MotorsMulticopter::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
return;
@ -283,6 +285,9 @@ void Copter::guided_takeoff_run()
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
}
// set motors to full range
motors.set_desired_spool_state(AP_MotorsMulticopter::DESIRED_THROTTLE_UNLIMITED);
// run waypoint controller
wp_nav.update_wpnav();
@ -298,12 +303,14 @@ void Copter::guided_takeoff_run()
void Copter::guided_pos_control_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
#else
motors.set_desired_spool_state(AP_MotorsMulticopter::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
return;
@ -319,6 +326,9 @@ void Copter::guided_pos_control_run()
}
}
// set motors to full range
motors.set_desired_spool_state(AP_MotorsMulticopter::DESIRED_THROTTLE_UNLIMITED);
// run waypoint controller
wp_nav.update_wpnav();
@ -340,14 +350,16 @@ void Copter::guided_pos_control_run()
void Copter::guided_vel_control_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
// initialise velocity controller
pos_control.init_vel_controller_xyz();
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
#else
motors.set_desired_spool_state(AP_MotorsMulticopter::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
return;
@ -363,6 +375,9 @@ void Copter::guided_vel_control_run()
}
}
// set motors to full range
motors.set_desired_spool_state(AP_MotorsMulticopter::DESIRED_THROTTLE_UNLIMITED);
// set velocity to zero if no updates received for 3 seconds
uint32_t tnow = millis();
if (tnow - vel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !pos_control.get_desired_velocity().is_zero()) {
@ -387,7 +402,7 @@ void Copter::guided_vel_control_run()
void Copter::guided_posvel_control_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
// set target position and velocity to current position and velocity
pos_control.set_pos_target(inertial_nav.get_position());
pos_control.set_desired_velocity(Vector3f(0,0,0));
@ -395,7 +410,9 @@ void Copter::guided_posvel_control_run()
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
#else
motors.set_desired_spool_state(AP_MotorsMulticopter::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
return;
@ -412,6 +429,9 @@ void Copter::guided_posvel_control_run()
}
}
// set motors to full range
motors.set_desired_spool_state(AP_MotorsMulticopter::DESIRED_THROTTLE_UNLIMITED);
// set velocity to zero if no updates received for 3 seconds
uint32_t tnow = millis();
if (tnow - posvel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !posvel_vel_target_cms.is_zero()) {
@ -456,13 +476,15 @@ void Copter::guided_posvel_control_run()
void Copter::guided_angle_control_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.set_yaw_target_to_current_heading();
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0.0f, 0.0f, 0.0f, get_smoothing_gain());
attitude_control.set_throttle_out(0.0f,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
#else
motors.set_desired_spool_state(AP_MotorsMulticopter::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0.0f,true,g.throttle_filt);
#endif
pos_control.relax_alt_hold_controllers(0.0f);
@ -494,6 +516,9 @@ void Copter::guided_angle_control_run()
climb_rate_cms = 0.0f;
}
// set motors to full range
motors.set_desired_spool_state(AP_MotorsMulticopter::DESIRED_THROTTLE_UNLIMITED);
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true);