Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 01:33:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: fixed LOITER_TURNS command

Browse Source 
and cleanup more loiter variables
This commit is contained in:
Andrew Tridgell 2013-04-15 21:31:11 +10:00
parent 9ff0888a17
commit 75f4a43e2d
3 changed files with 33 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
ArduPlane/ArduPlane.pde
View File

@ -459,15 +459,6 @@ static bool throttle_suppressed;
// Loiter management
////////////////////////////////////////////////////////////////////////////////
// Direction for loiter. 1 for clockwise, -1 for counter-clockwise
static int8_t loiter_direction = 1;
// The amount of time we have been in a Loiter. Used for the Loiter Time command. Milliseconds.
static uint32_t loiter_time_ms;
// The amount of time we should stay in a loiter for the Loiter Time command. Milliseconds.
static uint32_t loiter_time_max_ms;
////////////////////////////////////////////////////////////////////////////////
// Navigation control variables
////////////////////////////////////////////////////////////////////////////////
@ -491,11 +482,23 @@ static uint32_t wp_totalDistance;
meta data to support counting the number of circles in a loiter
*/
static struct {
// previous target bearing, used to update sum_cd
int32_t old_target_bearing_cd;
int32_t loiter_sum_cd;
// Total desired rotation in a loiter. Used for Loiter Turns commands.
int32_t loiter_total_cd;
int32_t total_cd;
// total angle completed in the loiter so far
int32_t sum_cd;
// Direction for loiter. 1 for clockwise, -1 for counter-clockwise
int8_t direction;
// start time of the loiter. Milliseconds.
uint32_t start_time_ms;
// The amount of time we should stay in a loiter for the Loiter Time command. Milliseconds.
uint32_t time_max_ms;
} loiter;

24
ArduPlane/commands_logic.pde
View File

@ -236,9 +236,9 @@ static void do_RTL(void)
next_WP = home;
if (g.loiter_radius < 0) {
loiter_direction = -1;
loiter.direction = -1;
} else {
loiter_direction = 1;
loiter.direction = 1;
}
// Altitude to hold over home
@ -275,9 +275,9 @@ static void do_land()
static void loiter_set_direction_wp(const struct Location *nav_command)
{
if (nav_command->options & MASK_OPTIONS_LOITER_DIRECTION) {
loiter_direction = -1;
loiter.direction = -1;
} else {
loiter_direction = 1;
loiter.direction = 1;
}
}
@ -290,15 +290,15 @@ static void do_loiter_unlimited()
static void do_loiter_turns()
{
set_next_WP(&next_nav_command);
loiter.loiter_total_cd = next_nav_command.p1 * 36000UL;
loiter.total_cd = next_nav_command.p1 * 36000UL;
loiter_set_direction_wp(&next_nav_command);
}
static void do_loiter_time()
{
set_next_WP(&next_nav_command);
loiter_time_ms = millis();
loiter_time_max_ms = next_nav_command.p1 * (uint32_t)1000; // units are seconds
loiter.start_time_ms = millis();
loiter.time_max_ms = next_nav_command.p1 * (uint32_t)1000; // units are seconds
loiter_set_direction_wp(&next_nav_command);
}
@ -413,7 +413,7 @@ static bool verify_loiter_unlim()
static bool verify_loiter_time()
{
update_loiter();
if ((millis() - loiter_time_ms) > loiter_time_max_ms) {
if ((millis() - loiter.start_time_ms) > loiter.time_max_ms) {
gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER time complete"));
return true;
}
@ -423,8 +423,8 @@ static bool verify_loiter_time()
static bool verify_loiter_turns()
{
update_loiter();
if (loiter.loiter_sum_cd > loiter.loiter_total_cd) {
loiter.loiter_total_cd = 0;
if (loiter.sum_cd > loiter.total_cd) {
loiter.total_cd = 0;
gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER orbits complete"));
// clear the command queue;
return true;
@ -511,9 +511,9 @@ static bool verify_within_distance()
static void do_loiter_at_location()
{
if (g.loiter_radius < 0) {
loiter_direction = -1;
loiter.direction = -1;
} else {
loiter_direction = 1;
loiter.direction = 1;
}
next_WP = current_loc;
}

13
ArduPlane/navigation.pde
View File

@ -16,8 +16,8 @@ static void set_nav_controller(void)
*/
static void loiter_angle_reset(void)
{
loiter.loiter_sum_cd = 0;
loiter.loiter_total_cd = 0;
loiter.sum_cd = 0;
loiter.total_cd = 0;
}
/*
@ -28,15 +28,16 @@ static void loiter_angle_update(void)
{
int32_t target_bearing_cd = nav_controller->target_bearing_cd();
int32_t loiter_delta_cd;
if (loiter.loiter_sum_cd == 0) {
loiter_delta_cd = 0;
if (loiter.sum_cd == 0) {
// use 1 cd for initial delta
loiter_delta_cd = 1;
} else {
loiter_delta_cd = target_bearing_cd - loiter.old_target_bearing_cd;
}
loiter.old_target_bearing_cd = target_bearing_cd;
loiter_delta_cd = wrap_180_cd(loiter_delta_cd);
loiter.loiter_sum_cd += loiter_delta_cd;
loiter.sum_cd += loiter_delta_cd;
}
//****************************************************************
@ -142,6 +143,6 @@ static void calc_altitude_error()
static void update_loiter()
{
nav_controller->update_loiter(next_WP, abs(g.loiter_radius), loiter_direction);
nav_controller->update_loiter(next_WP, abs(g.loiter_radius), loiter.direction);
}