|
|
|
@ -249,9 +249,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
@@ -249,9 +249,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
|
|
|
|
|
|
|
|
|
|
switch (message.compid) |
|
|
|
|
{ |
|
|
|
|
case MAV_COMP_ID_IMU_2: |
|
|
|
|
case MAV_COMP_ID_IMU: |
|
|
|
|
// Prefer IMU 2 over IMU 1 (FIXME)
|
|
|
|
|
componentID[message.msgid] = MAV_COMP_ID_IMU_2; |
|
|
|
|
componentID[message.msgid] = MAV_COMP_ID_IMU; |
|
|
|
|
break; |
|
|
|
|
default: |
|
|
|
|
// Do nothing
|
|
|
|
@ -2236,7 +2236,7 @@ void UAS::go()
@@ -2236,7 +2236,7 @@ void UAS::go()
|
|
|
|
|
sendMessage(msg); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/** Order the robot to return home / to land on the runway **/ |
|
|
|
|
/** Order the robot to return home **/ |
|
|
|
|
void UAS::home() |
|
|
|
|
{ |
|
|
|
|
mavlink_message_t msg; |
|
|
|
@ -2250,6 +2250,15 @@ void UAS::home()
@@ -2250,6 +2250,15 @@ void UAS::home()
|
|
|
|
|
sendMessage(msg); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/** Order the robot to land on the runway **/ |
|
|
|
|
void UAS::land() |
|
|
|
|
{ |
|
|
|
|
mavlink_message_t msg; |
|
|
|
|
|
|
|
|
|
mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_NAV_LAND, 1, 0, 0, 0, 0, 0, 0, 0); |
|
|
|
|
sendMessage(msg); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* The MAV starts the emergency landing procedure. The behaviour depends on the onboard implementation |
|
|
|
|
* and might differ between systems. |
|
|
|
|