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

Copter: control_stabilize, - integrate changes to get_throttle_surface_tracking

Browse Source
This commit is contained in:
Randy Mackay 2014-01-24 12:30:00 +09:00 committed by Andrew Tridgell
parent f3bcbb2ce4
commit c92de71212
2 changed files with 32 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
ArduCopter/control_auto.pde
View File

@ -7,12 +7,11 @@
// auto_init - initialise auto controller
static bool auto_init(bool ignore_checks)
{
if (GPS_ok() || ignore_checks) {
if ((GPS_ok() && g.command_total > 1) || ignore_checks) {
// set target to current position
wp_nav.init_loiter_target();
// initialise auto_yaw_mode
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
cliSerial->printf_P(PSTR("\nYM:%d\n"),(int)auto_yaw_mode);
// clear the command queues. will be reloaded when "run_autopilot" calls "update_commands" function
init_commands();
return true;
@ -169,7 +168,11 @@ float get_auto_heading(void)
// guided_init - initialise guided controller
static bool guided_init(bool ignore_checks)
{
return true;
if (GPS_ok() || ignore_checks) {
return true;
}else{
return false;
}
}
// guided_run - runs the guided controller
@ -194,7 +197,12 @@ static void land_run()
// rtl_init - initialise rtl controller
static bool rtl_init(bool ignore_checks)
{
return true;
if (GPS_ok() || ignore_checks) {
do_RTL();
return true;
}else{
return false;
}
}
// rtl_run - runs the return-to-launch controller

30
ArduCopter/control_stabilize.pde
View File

@ -170,11 +170,12 @@ static void althold_run()
// call throttle controller
if (sonar_alt_health >= SONAR_ALT_HEALTH_MAX) {
// if sonar is ok, use surface tracking
get_throttle_surface_tracking(target_climb_rate);
}else{
// if no sonar fall back stabilize rate controller
pos_control.climb_at_rate(target_climb_rate);
target_climb_rate = get_throttle_surface_tracking(target_climb_rate, G_Dt);
}
// call position controller
pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
pos_control.update_z_controller();
}
// refetch angle targets for reporting
@ -187,12 +188,16 @@ static void althold_run()
// circle_init - initialise circle controller
static bool circle_init(bool ignore_checks)
{
if (GPS_ok() || ignore_checks) {
return true;
}else{
return false;
}
// set yaw to point to center of circle
// yaw_look_at_WP = circle_center;
// initialise bearing to current heading
//yaw_look_at_WP_bearing = ahrs.yaw_sensor;
//yaw_initialised = true;
return true;
}
// circle_run - runs the circle controller
@ -275,11 +280,12 @@ static void loiter_run()
// run altitude controller
if (sonar_alt_health >= SONAR_ALT_HEALTH_MAX) {
// if sonar is ok, use surface tracking
get_throttle_surface_tracking(target_climb_rate);
}else{
// if no sonar fall back stabilize rate controller
pos_control.climb_at_rate(target_climb_rate);
target_climb_rate = get_throttle_surface_tracking(target_climb_rate,G_Dt);
}
// update altitude target and call position controller
pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
pos_control.update_z_controller();
}
// refetch angle targets for reporting
@ -292,7 +298,11 @@ static void loiter_run()
// ofloiter_init - initialise ofloiter controller
static bool ofloiter_init(bool ignore_checks)
{
return true;
if (g.optflow_enabled || ignore_checks) {
return true;
}else{
return false;
}
}
// ofloiter_run - runs the optical flow loiter controller