aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: use common location options masks

Browse Source
This commit is contained in:
Randy Mackay 2014-02-28 15:56:31 +09:00
parent 9ef27955b5
commit e88f1953d0
5 changed files with 15 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
ArduPlane/GCS_Mavlink.pde
View File

@ -1440,7 +1440,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// set frame of waypoint
uint8_t frame;
if (tell_command.options & MASK_OPTIONS_RELATIVE_ALT) {
if (tell_command.options & LOCATION_MASK_OPTIONS_RELATIVE_ALT) {
frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; // reference frame
} else {
frame = MAV_FRAME_GLOBAL; // reference frame
@ -1469,7 +1469,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAV_CMD_NAV_LOITER_TIME:
case MAV_CMD_NAV_LOITER_TURNS:
if (tell_command.options & MASK_OPTIONS_LOITER_DIRECTION) {
if (tell_command.options & LOCATION_MASK_OPTIONS_RELATIVE_ALT) {
param3 = -abs(g.loiter_radius);
} else {
param3 = abs(g.loiter_radius);
@ -1480,7 +1480,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
case MAV_CMD_NAV_LOITER_UNLIM:
if (tell_command.options & MASK_OPTIONS_LOITER_DIRECTION) {
if (tell_command.options & LOCATION_MASK_OPTIONS_RELATIVE_ALT) {
param3 = -abs(g.loiter_radius);
} else {
param3 = abs(g.loiter_radius);
@ -1741,7 +1741,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
(RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat;
tell_command.lng = 1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng;
tell_command.alt = -packet.z*1.0e2f;
tell_command.options = MASK_OPTIONS_RELATIVE_ALT;
tell_command.options = LOCATION_MASK_OPTIONS_RELATIVE_ALT;
break;
}
#endif
@ -1753,7 +1753,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
(RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat;
tell_command.lng = 1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng;
tell_command.alt = packet.z*1.0e2f;
tell_command.options = MASK_OPTIONS_RELATIVE_ALT;
tell_command.options = LOCATION_MASK_OPTIONS_RELATIVE_ALT;
break;
}
#endif
@ -1763,7 +1763,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
tell_command.lat = 1.0e7f * packet.x; // in as DD converted to * t7
tell_command.lng = 1.0e7f * packet.y; // in as DD converted to * t7
tell_command.alt = packet.z * 1.0e2f;
tell_command.options = MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!!
tell_command.options = LOCATION_MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!!
break;
}
@ -1779,7 +1779,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
switch (tell_command.id) {
case MAV_CMD_NAV_LOITER_UNLIM:
if (packet.param3 < 0) {
tell_command.options |= MASK_OPTIONS_LOITER_DIRECTION;
tell_command.options |= LOCATION_MASK_OPTIONS_RELATIVE_ALT;
}
case MAV_CMD_NAV_WAYPOINT:
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
@ -1789,7 +1789,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAV_CMD_NAV_LOITER_TURNS:
case MAV_CMD_NAV_LOITER_TIME:
if (packet.param3 < 0) {
tell_command.options |= MASK_OPTIONS_LOITER_DIRECTION;
tell_command.options |= LOCATION_MASK_OPTIONS_RELATIVE_ALT;
}
case MAV_CMD_NAV_TAKEOFF:
case MAV_CMD_DO_SET_HOME:
@ -1855,7 +1855,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
guided_WP = tell_command;
// add home alt if needed
if (guided_WP.options & MASK_OPTIONS_RELATIVE_ALT) {
if (guided_WP.options & LOCATION_MASK_OPTIONS_RELATIVE_ALT) {
guided_WP.alt += home.alt;
}
@ -1874,7 +1874,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
} else if(packet.current == 3) { //current = 3 is a flag to tell us this is a alt change only
// add home alt if needed
if (tell_command.options & MASK_OPTIONS_RELATIVE_ALT) {
if (tell_command.options & LOCATION_MASK_OPTIONS_RELATIVE_ALT) {
tell_command.alt += home.alt;
}

6
ArduPlane/commands.pde
View File

@ -80,7 +80,7 @@ static struct Location get_cmd_with_index(int16_t i)
// Add on home altitude if we are a nav command (or other command with altitude) and stored alt is relative
if ((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) &&
(temp.options & MASK_OPTIONS_RELATIVE_ALT) &&
(temp.options & LOCATION_MASK_OPTIONS_RELATIVE_ALT) &&
(temp.lat != 0 || temp.lng != 0 || temp.alt != 0)) {
temp.alt += home.alt;
}
@ -97,10 +97,10 @@ static void set_cmd_with_index(struct Location temp, int16_t i)
// force home wp to absolute height
if (i == 0) {
temp.options &= ~(MASK_OPTIONS_RELATIVE_ALT);
temp.options &= ~(LOCATION_MASK_OPTIONS_RELATIVE_ALT);
}
// zero unused bits
temp.options &= (MASK_OPTIONS_RELATIVE_ALT | MASK_OPTIONS_LOITER_DIRECTION);
temp.options &= (LOCATION_MASK_OPTIONS_RELATIVE_ALT | LOCATION_MASK_OPTIONS_LOITER_DIRECTION);
hal.storage->write_byte(mem, temp.id);

2
ArduPlane/commands_logic.pde
View File

@ -274,7 +274,7 @@ static void do_land()
static void loiter_set_direction_wp(const struct Location *nav_command)
{
if (nav_command->options & MASK_OPTIONS_LOITER_DIRECTION) {
if (nav_command->options & LOCATION_MASK_OPTIONS_LOITER_DIRECTION) {
loiter.direction = -1;
} else {
loiter.direction = 1;

7
ArduPlane/defines.h
View File

@ -102,13 +102,6 @@ enum ChannelMixing {
#define NO_COMMAND 0
#define WAIT_COMMAND 255
// Command/Waypoint/Location Options Bitmask
//--------------------
#define MASK_OPTIONS_RELATIVE_ALT (1<<0) // 1 = Relative
// altitude
#define MASK_OPTIONS_LOITER_DIRECTION (1<<2) // 0 = CW
// 1 = CCW
//repeating events
#define NO_REPEAT 0

2
ArduPlane/rally.pde
View File

@ -68,7 +68,7 @@ static Location rally_location_to_location(const RallyLocation &r_loc, const Loc
Location ret = {};
ret.id = MAV_CMD_NAV_LOITER_UNLIM;
ret.options = MASK_OPTIONS_RELATIVE_ALT;
ret.options = LOCATION_MASK_OPTIONS_RELATIVE_ALT;
//Currently can't do true AGL on the APM. Relative altitudes are
//relative to HOME point's altitude. Terrain on the board is inbound