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

Copter: add comments to auto_loiter

Browse Source
This commit is contained in:
Randy Mackay 2014-10-20 10:54:57 +09:00
parent d7d8330303
commit 3296eb24b3
1 changed files with 13 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
ArduCopter/control_auto.pde
View File

@ -421,8 +421,11 @@ void auto_nav_guided_run()
} }
#endif // NAV_GUIDED #endif // NAV_GUIDED
// auto_loiter_start - initialises loitering in auto mode
// returns success/failure because this can be called by exit_mission
bool auto_loiter_start() bool auto_loiter_start()
{ {
// return failure if GPS is bad
if (!GPS_ok()) { if (!GPS_ok()) {
return false; return false;
} }
@ -430,18 +433,25 @@ bool auto_loiter_start()
Vector3f origin = inertial_nav.get_position(); Vector3f origin = inertial_nav.get_position();
// calculate stopping point
Vector3f stopping_point; Vector3f stopping_point;
pos_control.get_stopping_point_xy(stopping_point); pos_control.get_stopping_point_xy(stopping_point);
pos_control.get_stopping_point_z(stopping_point); pos_control.get_stopping_point_z(stopping_point);
// initialise waypoint controller target to stopping point
wp_nav.set_wp_origin_and_destination(origin, stopping_point); wp_nav.set_wp_origin_and_destination(origin, stopping_point);
// hold yaw at current heading
set_auto_yaw_mode(AUTO_YAW_HOLD); set_auto_yaw_mode(AUTO_YAW_HOLD);
return true; return true;
} }
void auto_loiter_run() { // auto_loiter_run - loiter in AUTO flight mode
// called by auto_run at 100hz or more
void auto_loiter_run()
{
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete) { if(!ap.auto_armed || ap.land_complete) {
attitude_control.relax_bf_rate_controller(); attitude_control.relax_bf_rate_controller();
attitude_control.set_yaw_target_to_current_heading(); attitude_control.set_yaw_target_to_current_heading();
@ -449,11 +459,13 @@ void auto_loiter_run() {
return; return;
} }
// accept pilot input of yaw
float target_yaw_rate = 0; float target_yaw_rate = 0;
if(!failsafe.radio) { if(!failsafe.radio) {
target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in); target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in);
} }
// run waypoint and z-axis postion controller
wp_nav.update_wpnav(); wp_nav.update_wpnav();
pos_control.update_z_controller(); pos_control.update_z_controller();
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);