Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: use zero_throttle_and_relax_ac function

Browse Source
This commit is contained in:
Peter Barker 2016-11-17 16:47:41 +11:00 committed by Francisco Ferreira
parent 41dc8554c0
commit 71ad1b5815
6 changed files with 17 additions and 143 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
ArduCopter/mode_auto.cpp
View File

@ -153,15 +153,7 @@ void Copter::ModeAuto::takeoff_run()
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) { if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
// initialise wpnav targets // initialise wpnav targets
wp_nav->shift_wp_origin_to_current_pos(); wp_nav->shift_wp_origin_to_current_pos();
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw zero_throttle_and_relax_ac();
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(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
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// reset attitude control targets
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// clear i term when we're taking off // clear i term when we're taking off
set_throttle_takeoff(); set_throttle_takeoff();
return; return;
@ -241,14 +233,7 @@ void Copter::ModeAuto::wp_run()
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) { if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
// To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off // To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off
// (of course it would be better if people just used take-off) // (of course it would be better if people just used take-off)
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw zero_throttle_and_relax_ac();
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(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
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// clear i term when we're taking off // clear i term when we're taking off
set_throttle_takeoff(); set_throttle_takeoff();
return; return;
@ -313,14 +298,7 @@ void Copter::ModeAuto::spline_run()
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) { if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
// To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off // To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off
// (of course it would be better if people just used take-off) // (of course it would be better if people just used take-off)
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw zero_throttle_and_relax_ac();
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(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
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
#endif
// clear i term when we're taking off // clear i term when we're taking off
set_throttle_takeoff(); set_throttle_takeoff();
return; return;
@ -390,15 +368,7 @@ void Copter::ModeAuto::land_run()
{ {
// if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) { if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw zero_throttle_and_relax_ac();
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control->set_throttle_out(0,false,g.throttle_filt);
#else
motors->set_desired_spool_state(AP_Motors::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
// set target to current position // set target to current position
wp_nav->init_loiter_target(); wp_nav->init_loiter_target();
return; return;
@ -570,15 +540,7 @@ void Copter::ModeAuto::loiter_run()
{ {
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) { if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw zero_throttle_and_relax_ac();
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control->set_throttle_out(0,false,g.throttle_filt);
#else
motors->set_desired_spool_state(AP_Motors::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; return;
} }

10
ArduCopter/mode_brake.cpp
View File

@ -40,15 +40,7 @@ void Copter::ModeBrake::run()
// if not auto armed set throttle to zero and exit immediately // if not auto armed set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) { if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
wp_nav->init_brake_target(BRAKE_MODE_DECEL_RATE); wp_nav->init_brake_target(BRAKE_MODE_DECEL_RATE);
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw zero_throttle_and_relax_ac();
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control->set_throttle_out(0,false,g.throttle_filt);
#else
motors->set_desired_spool_state(AP_Motors::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
pos_control->relax_alt_hold_controllers(0.0f); pos_control->relax_alt_hold_controllers(0.0f);
return; return;
} }

10
ArduCopter/mode_circle.cpp
View File

@ -42,15 +42,7 @@ void Copter::ModeCircle::run()
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) { if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {
// To-Do: add some initialisation of position controllers // To-Do: add some initialisation of position controllers
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw zero_throttle_and_relax_ac();
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control->set_throttle_out(0,false,g.throttle_filt);
#else
motors->set_desired_spool_state(AP_Motors::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
pos_control->set_alt_target_to_current_alt(); pos_control->set_alt_target_to_current_alt();
return; return;
} }

40
ArduCopter/mode_guided.cpp
View File

@ -415,15 +415,7 @@ void Copter::ModeGuided::pos_control_run()
{ {
// if not auto armed or motors not enabled set throttle to zero and exit immediately // if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!motors->armed() || !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 zero_throttle_and_relax_ac();
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control->set_throttle_out(0,false,g.throttle_filt);
#else
motors->set_desired_spool_state(AP_Motors::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; return;
} }
@ -467,15 +459,7 @@ void Copter::ModeGuided::vel_control_run()
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) { if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) {
// initialise velocity controller // initialise velocity controller
pos_control->init_vel_controller_xyz(); pos_control->init_vel_controller_xyz();
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw zero_throttle_and_relax_ac();
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control->set_throttle_out(0,false,g.throttle_filt);
#else
motors->set_desired_spool_state(AP_Motors::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; return;
} }
@ -530,15 +514,7 @@ void Copter::ModeGuided::posvel_control_run()
// set target position and velocity to current position and velocity // set target position and velocity to current position and velocity
pos_control->set_pos_target(inertial_nav.get_position()); pos_control->set_pos_target(inertial_nav.get_position());
pos_control->set_desired_velocity(Vector3f(0,0,0)); pos_control->set_desired_velocity(Vector3f(0,0,0));
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw zero_throttle_and_relax_ac();
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control->set_throttle_out(0,false,g.throttle_filt);
#else
motors->set_desired_spool_state(AP_Motors::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; return;
} }
@ -607,16 +583,10 @@ void Copter::ModeGuided::angle_control_run()
{ {
// if not auto armed or motors not enabled set throttle to zero and exit immediately // if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || (ap.land_complete && guided_angle_state.climb_rate_cms <= 0.0f)) { if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || (ap.land_complete && guided_angle_state.climb_rate_cms <= 0.0f)) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw #if FRAME_CONFIG == HELI_FRAME
// call attitude controller
attitude_control->set_yaw_target_to_current_heading(); attitude_control->set_yaw_target_to_current_heading();
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f, get_smoothing_gain());
attitude_control->set_throttle_out(0.0f,false,g.throttle_filt);
#else
motors->set_desired_spool_state(AP_Motors::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 #endif
zero_throttle_and_relax_ac();
pos_control->relax_alt_hold_controllers(0.0f); pos_control->relax_alt_hold_controllers(0.0f);
return; return;
} }

10
ArduCopter/mode_land.cpp
View File

@ -56,15 +56,7 @@ void Copter::ModeLand::gps_run()
{ {
// if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) { if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw zero_throttle_and_relax_ac();
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control->set_throttle_out(0,false,g.throttle_filt);
#else
motors->set_desired_spool_state(AP_Motors::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
wp_nav->init_loiter_target(); wp_nav->init_loiter_target();
// disarm when the landing detector says we've landed // disarm when the landing detector says we've landed

42
ArduCopter/mode_rtl.cpp
View File

@ -132,16 +132,7 @@ void Copter::ModeRTL::climb_return_run()
{ {
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors->armed() || !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 zero_throttle_and_relax_ac();
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control->set_throttle_out(0,false,g.throttle_filt);
#else
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
// reset attitude control targets
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// To-Do: re-initialise wpnav targets // To-Do: re-initialise wpnav targets
return; return;
} }
@ -199,16 +190,7 @@ void Copter::ModeRTL::loiterathome_run()
{ {
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors->armed() || !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 zero_throttle_and_relax_ac();
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control->set_throttle_out(0,false,g.throttle_filt);
#else
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
// reset attitude control targets
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// To-Do: re-initialise wpnav targets // To-Do: re-initialise wpnav targets
return; return;
} }
@ -280,15 +262,7 @@ void Copter::ModeRTL::descent_run()
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors->armed() || !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 zero_throttle_and_relax_ac();
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control->set_throttle_out(0,false,g.throttle_filt);
#else
motors->set_desired_spool_state(AP_Motors::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
// set target to current position // set target to current position
wp_nav->init_loiter_target(); wp_nav->init_loiter_target();
return; return;
@ -375,15 +349,7 @@ void Copter::ModeRTL::land_run(bool disarm_on_land)
{ {
// if not auto armed or landing completed or motor interlock not enabled set throttle to zero and exit immediately // if not auto armed or landing completed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) { if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw zero_throttle_and_relax_ac();
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control->set_throttle_out(0,false,g.throttle_filt);
#else
motors->set_desired_spool_state(AP_Motors::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
// set target to current position // set target to current position
wp_nav->init_loiter_target(); wp_nav->init_loiter_target();