zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Copter: use zero_throttle_and_relax_ac function

master
Peter Barker 8 years ago committed by Francisco Ferreira
parent
41dc8554c0
commit
71ad1b5815
6 changed files with 17 additions and 143 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. 48
      ArduCopter/mode_auto.cpp
  2. 10
      ArduCopter/mode_brake.cpp
  3. 10
      ArduCopter/mode_circle.cpp
  4. 40
      ArduCopter/mode_guided.cpp
  5. 10
      ArduCopter/mode_land.cpp
  6. 42
      ArduCopter/mode_rtl.cpp

48
ArduCopter/mode_auto.cpp
Unescape View File

@ -153,15 +153,7 @@ void Copter::ModeAuto::takeoff_run() @@ -153,15 +153,7 @@ void Copter::ModeAuto::takeoff_run()
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
// initialise wpnav targets
wp_nav->shift_wp_origin_to_current_pos();
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// 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
zero_throttle_and_relax_ac();
// clear i term when we're taking off
set_throttle_takeoff();
return;
@ -241,14 +233,7 @@ void Copter::ModeAuto::wp_run() @@ -241,14 +233,7 @@ void Copter::ModeAuto::wp_run()
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
// (of course it would be better if people just used take-off)
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// 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
zero_throttle_and_relax_ac();
// clear i term when we're taking off
set_throttle_takeoff();
return;
@ -313,14 +298,7 @@ void Copter::ModeAuto::spline_run() @@ -313,14 +298,7 @@ void Copter::ModeAuto::spline_run()
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
// (of course it would be better if people just used take-off)
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// 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
zero_throttle_and_relax_ac();
// clear i term when we're taking off
set_throttle_takeoff();
return;
@ -390,15 +368,7 @@ void Copter::ModeAuto::land_run() @@ -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 (!motors->armed() || !ap.auto_armed || ap.land_complete || !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(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
zero_throttle_and_relax_ac();
// set target to current position
wp_nav->init_loiter_target();
return;
@ -570,15 +540,7 @@ void Copter::ModeAuto::loiter_run() @@ -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 (!motors->armed() || !ap.auto_armed || ap.land_complete || !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(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
zero_throttle_and_relax_ac();
return;
}

10
ArduCopter/mode_brake.cpp
Unescape View File

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

10
ArduCopter/mode_circle.cpp
Unescape View File

@ -42,15 +42,7 @@ void Copter::ModeCircle::run() @@ -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 (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {
// To-Do: add some initialisation of position controllers
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// 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
zero_throttle_and_relax_ac();
pos_control->set_alt_target_to_current_alt();
return;
}

40
ArduCopter/mode_guided.cpp
Unescape View File

@ -415,15 +415,7 @@ void Copter::ModeGuided::pos_control_run() @@ -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 (!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(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
zero_throttle_and_relax_ac();
return;
}
@ -467,15 +459,7 @@ void Copter::ModeGuided::vel_control_run() @@ -467,15 +459,7 @@ void Copter::ModeGuided::vel_control_run()
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(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
zero_throttle_and_relax_ac();
return;
}
@ -530,15 +514,7 @@ void Copter::ModeGuided::posvel_control_run() @@ -530,15 +514,7 @@ void Copter::ModeGuided::posvel_control_run()
// 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));
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// 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
zero_throttle_and_relax_ac();
return;
}
@ -607,16 +583,10 @@ void Copter::ModeGuided::angle_control_run() @@ -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 (!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
// call attitude controller
#if FRAME_CONFIG == HELI_FRAME
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
zero_throttle_and_relax_ac();
pos_control->relax_alt_hold_controllers(0.0f);
return;
}

10
ArduCopter/mode_land.cpp
Unescape View File

@ -56,15 +56,7 @@ void Copter::ModeLand::gps_run() @@ -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 (!motors->armed() || !ap.auto_armed || ap.land_complete || !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(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
zero_throttle_and_relax_ac();
wp_nav->init_loiter_target();
// disarm when the landing detector says we've landed

42
ArduCopter/mode_rtl.cpp
Unescape View File

@ -132,16 +132,7 @@ void Copter::ModeRTL::climb_return_run() @@ -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 (!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(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
zero_throttle_and_relax_ac();
// To-Do: re-initialise wpnav targets
return;
}
@ -199,16 +190,7 @@ void Copter::ModeRTL::loiterathome_run() @@ -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 (!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(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
zero_throttle_and_relax_ac();
// To-Do: re-initialise wpnav targets
return;
}
@ -280,15 +262,7 @@ void Copter::ModeRTL::descent_run() @@ -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 (!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(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
zero_throttle_and_relax_ac();
// set target to current position
wp_nav->init_loiter_target();
return;
@ -375,15 +349,7 @@ void Copter::ModeRTL::land_run(bool disarm_on_land) @@ -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 (!motors->armed() || !ap.auto_armed || ap.land_complete || !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(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
zero_throttle_and_relax_ac();
// set target to current position
wp_nav->init_loiter_target();

Write Preview
Loading…
Cancel
Save