Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

APM: fixed LOITER_TIME and LOITER_TURNS 

These now follow the mavlink spec. Loiter time is in seconds, and
loiter turns is now 32 bit angle, so can handle larger numbers of
turns.
This commit is contained in:
Andrew Tridgell 2012-08-12 20:53:44 +10:00
parent 57cc89b730
commit 93f1d5645b
3 changed files with 8 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduPlane/ArduPlane.pde
View File

@ -515,13 +515,13 @@ static int16_t takeoff_pitch_cd;
static int32_t old_target_bearing_cd;
// Total desired rotation in a loiter. Used for Loiter Turns commands. Degrees
static int16_t loiter_total;
static int32_t loiter_total;
// The amount in degrees we have turned since recording old_target_bearing
static int16_t loiter_delta;
// Total rotation in a loiter. Used for Loiter Turns commands and to check for missed waypoints. Degrees
static int16_t loiter_sum;
static int32_t loiter_sum;
// The amount of time we have been in a Loiter. Used for the Loiter Time command. Milliseconds.
static uint32_t loiter_time_ms;

13
ArduPlane/GCS_Mavlink.pde
View File

@ -1169,11 +1169,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAV_CMD_NAV_LOITER_TURNS:
case MAV_CMD_NAV_TAKEOFF:
case MAV_CMD_DO_SET_HOME:
param1 = tell_command.p1;
break;
case MAV_CMD_NAV_LOITER_TIME:
param1 = tell_command.p1*10; // APM loiter time is in ten second increments
param1 = tell_command.p1;
break;
case MAV_CMD_CONDITION_CHANGE_ALT:
@ -1408,7 +1405,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
if (result != MAV_MISSION_ACCEPTED) goto mission_failed;
switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields
// Switch to map APM command fields into MAVLink command fields
switch (tell_command.id) {
case MAV_CMD_NAV_WAYPOINT:
case MAV_CMD_NAV_LOITER_UNLIM:
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
@ -1418,6 +1416,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAV_CMD_NAV_LOITER_TURNS:
case MAV_CMD_NAV_TAKEOFF:
case MAV_CMD_DO_SET_HOME:
case MAV_CMD_NAV_LOITER_TIME:
tell_command.p1 = packet.param1;
break;
@ -1425,10 +1424,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
tell_command.lat = packet.param1;
break;
case MAV_CMD_NAV_LOITER_TIME:
tell_command.p1 = packet.param1 / 10; // APM loiter time is in ten second increments
break;
case MAV_CMD_CONDITION_DELAY:
case MAV_CMD_CONDITION_DISTANCE:
tell_command.lat = packet.param1;

12
ArduPlane/commands_logic.pde
View File

@ -161,37 +161,29 @@ static bool verify_nav_command() // Returns true if command complete
case MAV_CMD_NAV_TAKEOFF:
return verify_takeoff();
break;
case MAV_CMD_NAV_LAND:
return verify_land();
break;
case MAV_CMD_NAV_WAYPOINT:
return verify_nav_wp();
break;
case MAV_CMD_NAV_LOITER_UNLIM:
return verify_loiter_unlim();
break;
case MAV_CMD_NAV_LOITER_TURNS:
return verify_loiter_turns();
break;
case MAV_CMD_NAV_LOITER_TIME:
return verify_loiter_time();
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
return verify_RTL();
break;
default:
gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_nav: Invalid or no current Nav cmd"));
return false;
break;
}
return false;
}
static bool verify_condition_command() // Returns true if command complete
@ -283,7 +275,7 @@ 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)100; // units are (seconds * 10)
loiter_time_max_ms = next_nav_command.p1 * (uint32_t)1000; // units are seconds
}
/********************************************************************************/