MAVLink: use new MAV_MODE_FLAG_CUSTOM_MODE_ENABLED flag

this enables us to tell if custom_mode is set
This commit is contained in:
Andrew Tridgell 2011-10-30 12:41:32 +11:00
parent d74a223f7e
commit e6f7bf9df8
1 changed files with 14 additions and 16 deletions

View File

@ -28,16 +28,11 @@ static bool mavlink_active;
static NOINLINE void send_heartbeat(mavlink_channel_t chan) static NOINLINE void send_heartbeat(mavlink_channel_t chan)
{ {
#ifdef MAVLINK10 #ifdef MAVLINK10
uint8_t base_mode = 0; uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
uint8_t system_status = MAV_STATE_ACTIVE; uint8_t system_status = MAV_STATE_ACTIVE;
uint32_t custom_mode = control_mode;
// we map the custom_mode to our internal mode plus 16, to lower // work out the base_mode. This value is not very useful
// the chance that a ground station will give us 0 and we
// interpret it as manual. This is necessary as the SET_MODE
// command has no way to indicate that the custom_mode is filled in
uint32_t custom_mode = control_mode + 16;
// work out the base_mode. This value is almost completely useless
// for APM, but we calculate it as best we can so a generic // for APM, but we calculate it as best we can so a generic
// MAVLink enabled ground station can work out something about // MAVLink enabled ground station can work out something about
// what the MAV is up to. The actual bit values are highly // what the MAV is up to. The actual bit values are highly
@ -93,6 +88,9 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan)
base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
} }
// indicate we have set a custom mode
base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
mavlink_msg_heartbeat_send( mavlink_msg_heartbeat_send(
chan, chan,
MAV_TYPE_FIXED_WING, MAV_TYPE_FIXED_WING,
@ -1140,13 +1138,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
mavlink_msg_set_mode_decode(msg, &packet); mavlink_msg_set_mode_decode(msg, &packet);
#ifdef MAVLINK10 #ifdef MAVLINK10
// we ignore base_mode as there is no sane way to map if (!(packet.base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)) {
// from that bitmap to a APM flight mode. We rely on // we ignore base_mode as there is no sane way to map
// custom_mode instead. // from that bitmap to a APM flight mode. We rely on
// see comment on custom_mode above // custom_mode instead.
int16_t adjusted_mode = packet.custom_mode - 16; break;
}
switch (adjusted_mode) { switch (packet.custom_mode) {
case MANUAL: case MANUAL:
case CIRCLE: case CIRCLE:
case STABILIZE: case STABILIZE:
@ -1156,7 +1154,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case AUTO: case AUTO:
case RTL: case RTL:
case LOITER: case LOITER:
set_mode(adjusted_mode); set_mode(packet.custom_mode);
break; break;
} }