|
|
|
@ -281,7 +281,7 @@ void GCS_MAVLINK_Rover::send_pid_tuning()
@@ -281,7 +281,7 @@ void GCS_MAVLINK_Rover::send_pid_tuning()
|
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void Rover::send_wheel_encoder_distance(mavlink_channel_t chan) |
|
|
|
|
void Rover::send_wheel_encoder_distance(const mavlink_channel_t chan) |
|
|
|
|
{ |
|
|
|
|
// send wheel encoder data using wheel_distance message
|
|
|
|
|
if (g2.wheel_encoder.num_sensors() > 0) { |
|
|
|
@ -338,7 +338,7 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
@@ -338,7 +338,7 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
|
|
|
|
|
return true; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void GCS_MAVLINK_Rover::packetReceived(const mavlink_status_t &status, mavlink_message_t &msg) |
|
|
|
|
void GCS_MAVLINK_Rover::packetReceived(const mavlink_status_t &status, const mavlink_message_t &msg) |
|
|
|
|
{ |
|
|
|
|
// pass message to follow library
|
|
|
|
|
rover.g2.follow.handle_msg(msg); |
|
|
|
@ -653,27 +653,27 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_long_packet(const mavlink_command_l
@@ -653,27 +653,27 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_long_packet(const mavlink_command_l
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// a RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposes
|
|
|
|
|
void GCS_MAVLINK_Rover::handle_rc_channels_override(const mavlink_message_t *msg) |
|
|
|
|
void GCS_MAVLINK_Rover::handle_rc_channels_override(const mavlink_message_t &msg) |
|
|
|
|
{ |
|
|
|
|
rover.failsafe.last_heartbeat_ms = AP_HAL::millis(); |
|
|
|
|
GCS_MAVLINK::handle_rc_channels_override(msg); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) |
|
|
|
|
void GCS_MAVLINK_Rover::handleMessage(const mavlink_message_t &msg) |
|
|
|
|
{ |
|
|
|
|
switch (msg->msgid) { |
|
|
|
|
switch (msg.msgid) { |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
case MAVLINK_MSG_ID_MANUAL_CONTROL: |
|
|
|
|
{ |
|
|
|
|
if (msg->sysid != rover.g.sysid_my_gcs) { // Only accept control from our gcs
|
|
|
|
|
if (msg.sysid != rover.g.sysid_my_gcs) { // Only accept control from our gcs
|
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
mavlink_manual_control_t packet; |
|
|
|
|
mavlink_msg_manual_control_decode(msg, &packet); |
|
|
|
|
mavlink_msg_manual_control_decode(&msg, &packet); |
|
|
|
|
|
|
|
|
|
if (packet.target != rover.g.sysid_this_mav) { |
|
|
|
|
break; // only accept control aimed at us
|
|
|
|
@ -692,7 +692,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
@@ -692,7 +692,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
|
|
|
|
|
case MAVLINK_MSG_ID_HEARTBEAT: |
|
|
|
|
{ |
|
|
|
|
// we keep track of the last time we received a heartbeat from our GCS for failsafe purposes
|
|
|
|
|
if (msg->sysid != rover.g.sysid_my_gcs) { |
|
|
|
|
if (msg.sysid != rover.g.sysid_my_gcs) { |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
rover.failsafe.last_heartbeat_ms = AP_HAL::millis(); |
|
|
|
@ -703,7 +703,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
@@ -703,7 +703,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
|
|
|
|
|
{ |
|
|
|
|
// decode packet
|
|
|
|
|
mavlink_set_attitude_target_t packet; |
|
|
|
|
mavlink_msg_set_attitude_target_decode(msg, &packet); |
|
|
|
|
mavlink_msg_set_attitude_target_decode(&msg, &packet); |
|
|
|
|
|
|
|
|
|
// exit if vehicle is not in Guided mode
|
|
|
|
|
if (!rover.control_mode->in_guided_mode()) { |
|
|
|
@ -735,7 +735,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
@@ -735,7 +735,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
|
|
|
|
|
{ |
|
|
|
|
// decode packet
|
|
|
|
|
mavlink_set_position_target_local_ned_t packet; |
|
|
|
|
mavlink_msg_set_position_target_local_ned_decode(msg, &packet); |
|
|
|
|
mavlink_msg_set_position_target_local_ned_decode(&msg, &packet); |
|
|
|
|
|
|
|
|
|
// exit if vehicle is not in Guided mode
|
|
|
|
|
if (!rover.control_mode->in_guided_mode()) { |
|
|
|
@ -855,7 +855,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
@@ -855,7 +855,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
|
|
|
|
|
{ |
|
|
|
|
// decode packet
|
|
|
|
|
mavlink_set_position_target_global_int_t packet; |
|
|
|
|
mavlink_msg_set_position_target_global_int_decode(msg, &packet); |
|
|
|
|
mavlink_msg_set_position_target_global_int_decode(&msg, &packet); |
|
|
|
|
|
|
|
|
|
// exit if vehicle is not in Guided mode
|
|
|
|
|
if (!rover.control_mode->in_guided_mode()) { |
|
|
|
@ -960,7 +960,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
@@ -960,7 +960,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
|
|
|
|
|
case MAVLINK_MSG_ID_HIL_STATE: |
|
|
|
|
{ |
|
|
|
|
mavlink_hil_state_t packet; |
|
|
|
|
mavlink_msg_hil_state_decode(msg, &packet); |
|
|
|
|
mavlink_msg_hil_state_decode(&msg, &packet); |
|
|
|
|
|
|
|
|
|
// sanity check location
|
|
|
|
|
if (!check_latlng(packet.lat, packet.lon)) { |
|
|
|
|