|
|
|
@ -860,14 +860,6 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
@@ -860,14 +860,6 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
|
|
|
|
|
} |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: { |
|
|
|
|
if (is_equal(packet.param1, 1.0f)) { |
|
|
|
|
send_autopilot_version(FIRMWARE_VERSION); |
|
|
|
|
result = MAV_RESULT_ACCEPTED; |
|
|
|
|
} |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
case MAV_CMD_DO_SET_HOME: |
|
|
|
|
{ |
|
|
|
|
// param1 : use current (1=use current location, 0=use specified location)
|
|
|
|
@ -1278,10 +1270,6 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
@@ -1278,10 +1270,6 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
|
|
|
|
|
rover.rangefinder.handle_msg(msg); |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST: |
|
|
|
|
send_autopilot_version(FIRMWARE_VERSION); |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case MAVLINK_MSG_ID_VISION_POSITION_DELTA: |
|
|
|
|
rover.g2.visual_odom.handle_msg(msg); |
|
|
|
|
break; |
|
|
|
@ -1415,3 +1403,8 @@ bool GCS_MAVLINK_Rover::set_mode(const uint8_t mode)
@@ -1415,3 +1403,8 @@ bool GCS_MAVLINK_Rover::set_mode(const uint8_t mode)
|
|
|
|
|
} |
|
|
|
|
return rover.set_mode(*new_mode, MODE_REASON_GCS_COMMAND); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
const AP_FWVersion &GCS_MAVLINK_Rover::get_fwver() const |
|
|
|
|
{ |
|
|
|
|
return rover.fwver; |
|
|
|
|
} |
|
|
|
|