|
|
|
@ -1135,7 +1135,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
@@ -1135,7 +1135,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|
|
|
|
uint8_t result = MAV_RESULT_UNSUPPORTED; |
|
|
|
|
|
|
|
|
|
// do command
|
|
|
|
|
send_text(MAV_SEVERITY_INFO,"command received: "); |
|
|
|
|
send_text(MAV_SEVERITY_INFO,"Command received: "); |
|
|
|
|
|
|
|
|
|
switch(packet.command) { |
|
|
|
|
|
|
|
|
@ -1386,9 +1386,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
@@ -1386,9 +1386,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|
|
|
|
plane.auto_state.commanded_go_around = true; |
|
|
|
|
|
|
|
|
|
result = MAV_RESULT_ACCEPTED; |
|
|
|
|
plane.gcs_send_text(MAV_SEVERITY_INFO,"Go around command accepted."); |
|
|
|
|
plane.gcs_send_text(MAV_SEVERITY_INFO,"Go around command accepted"); |
|
|
|
|
} else { |
|
|
|
|
plane.gcs_send_text(MAV_SEVERITY_NOTICE,"Rejected go around command."); |
|
|
|
|
plane.gcs_send_text(MAV_SEVERITY_NOTICE,"Rejected go around command"); |
|
|
|
|
} |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
@ -1412,7 +1412,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
@@ -1412,7 +1412,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|
|
|
|
if (! plane.geofence_set_floor_enabled(false)) { |
|
|
|
|
result = MAV_RESULT_FAILED; |
|
|
|
|
} else { |
|
|
|
|
plane.gcs_send_text(MAV_SEVERITY_NOTICE,"Fence floor disabled."); |
|
|
|
|
plane.gcs_send_text(MAV_SEVERITY_NOTICE,"Fence floor disabled"); |
|
|
|
|
} |
|
|
|
|
break; |
|
|
|
|
default: |
|
|
|
@ -1454,7 +1454,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
@@ -1454,7 +1454,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|
|
|
|
plane.Log_Write_Home_And_Origin(); |
|
|
|
|
GCS_MAVLINK::send_home_all(new_home_loc); |
|
|
|
|
result = MAV_RESULT_ACCEPTED; |
|
|
|
|
plane.gcs_send_text_fmt(MAV_SEVERITY_INFO, "set home to %.6f %.6f at %um", |
|
|
|
|
plane.gcs_send_text_fmt(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %um", |
|
|
|
|
(double)(new_home_loc.lat*1.0e-7f), |
|
|
|
|
(double)(new_home_loc.lng*1.0e-7f), |
|
|
|
|
(uint32_t)(new_home_loc.alt*0.01f)); |
|
|
|
@ -1612,11 +1612,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
@@ -1612,11 +1612,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|
|
|
|
mavlink_fence_point_t packet; |
|
|
|
|
mavlink_msg_fence_point_decode(msg, &packet); |
|
|
|
|
if (plane.g.fence_action != FENCE_ACTION_NONE) { |
|
|
|
|
send_text(MAV_SEVERITY_WARNING,"fencing must be disabled"); |
|
|
|
|
send_text(MAV_SEVERITY_WARNING,"Fencing must be disabled"); |
|
|
|
|
} else if (packet.count != plane.g.fence_total) { |
|
|
|
|
send_text(MAV_SEVERITY_WARNING,"bad fence point"); |
|
|
|
|
send_text(MAV_SEVERITY_WARNING,"Bad fence point"); |
|
|
|
|
} else if (fabsf(packet.lat) > 90.0f || fabsf(packet.lng) > 180.0f) { |
|
|
|
|
send_text(MAV_SEVERITY_WARNING,"invalid fence point, lat or lng too large"); |
|
|
|
|
send_text(MAV_SEVERITY_WARNING,"Invalid fence point, lat or lng too large"); |
|
|
|
|
} else { |
|
|
|
|
Vector2l point; |
|
|
|
|
point.x = packet.lat*1.0e7f; |
|
|
|
@ -1631,7 +1631,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
@@ -1631,7 +1631,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|
|
|
|
mavlink_fence_fetch_point_t packet; |
|
|
|
|
mavlink_msg_fence_fetch_point_decode(msg, &packet); |
|
|
|
|
if (packet.idx >= plane.g.fence_total) { |
|
|
|
|
send_text(MAV_SEVERITY_WARNING,"bad fence point"); |
|
|
|
|
send_text(MAV_SEVERITY_WARNING,"Bad fence point"); |
|
|
|
|
} else { |
|
|
|
|
Vector2l point = plane.get_fence_point_with_index(packet.idx); |
|
|
|
|
mavlink_msg_fence_point_send_buf(msg, chan, msg->sysid, msg->compid, packet.idx, plane.g.fence_total, |
|
|
|
@ -1648,12 +1648,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
@@ -1648,12 +1648,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|
|
|
|
|
|
|
|
|
if (packet.idx >= plane.rally.get_rally_total() ||
|
|
|
|
|
packet.idx >= plane.rally.get_rally_max()) { |
|
|
|
|
send_text(MAV_SEVERITY_WARNING,"bad rally point message ID"); |
|
|
|
|
send_text(MAV_SEVERITY_WARNING,"Bad rally point message ID"); |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (packet.count != plane.rally.get_rally_total()) { |
|
|
|
|
send_text(MAV_SEVERITY_WARNING,"bad rally point message count"); |
|
|
|
|
send_text(MAV_SEVERITY_WARNING,"Bad rally point message count"); |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -1673,12 +1673,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
@@ -1673,12 +1673,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|
|
|
|
mavlink_rally_fetch_point_t packet; |
|
|
|
|
mavlink_msg_rally_fetch_point_decode(msg, &packet); |
|
|
|
|
if (packet.idx > plane.rally.get_rally_total()) { |
|
|
|
|
send_text(MAV_SEVERITY_WARNING, "bad rally point index"); |
|
|
|
|
send_text(MAV_SEVERITY_WARNING, "Bad rally point index"); |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
RallyLocation rally_point; |
|
|
|
|
if (!plane.rally.get_rally_point_with_index(packet.idx, rally_point)) { |
|
|
|
|
send_text(MAV_SEVERITY_WARNING, "failed to set rally point"); |
|
|
|
|
send_text(MAV_SEVERITY_WARNING, "Failed to set rally point"); |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -1893,7 +1893,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
@@ -1893,7 +1893,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|
|
|
|
plane.home_is_set = HOME_SET_NOT_LOCKED; |
|
|
|
|
plane.Log_Write_Home_And_Origin(); |
|
|
|
|
GCS_MAVLINK::send_home_all(new_home_loc); |
|
|
|
|
plane.gcs_send_text_fmt(MAV_SEVERITY_INFO, "set home to %.6f %.6f at %um", |
|
|
|
|
plane.gcs_send_text_fmt(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %um", |
|
|
|
|
(double)(new_home_loc.lat*1.0e-7f), |
|
|
|
|
(double)(new_home_loc.lng*1.0e-7f), |
|
|
|
|
(uint32_t)(new_home_loc.alt*0.01f)); |
|
|
|
|