mirror of https://github.com/ArduPilot/ardupilot
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:
parent
d74a223f7e
commit
e6f7bf9df8
|
@ -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
|
||||||
|
if (!(packet.base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)) {
|
||||||
// we ignore base_mode as there is no sane way to map
|
// we ignore base_mode as there is no sane way to map
|
||||||
// from that bitmap to a APM flight mode. We rely on
|
// from that bitmap to a APM flight mode. We rely on
|
||||||
// custom_mode instead.
|
// custom_mode instead.
|
||||||
// see comment on custom_mode above
|
break;
|
||||||
int16_t adjusted_mode = packet.custom_mode - 16;
|
}
|
||||||
|
switch (packet.custom_mode) {
|
||||||
switch (adjusted_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;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue