|
|
|
@ -168,7 +168,7 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
@@ -168,7 +168,7 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
|
|
|
|
|
switch (id) { |
|
|
|
|
case MSG_HEARTBEAT: |
|
|
|
|
CHECK_PAYLOAD_SIZE(HEARTBEAT); |
|
|
|
|
tracker.gcs[chan-MAVLINK_COMM_0].last_heartbeat_time = AP_HAL::millis(); |
|
|
|
|
last_heartbeat_time = AP_HAL::millis(); |
|
|
|
|
tracker.send_heartbeat(chan); |
|
|
|
|
return true; |
|
|
|
|
|
|
|
|
@ -194,12 +194,12 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
@@ -194,12 +194,12 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
|
|
|
|
|
|
|
|
|
|
case MSG_GPS_RAW: |
|
|
|
|
CHECK_PAYLOAD_SIZE(GPS_RAW_INT); |
|
|
|
|
tracker.gcs[chan-MAVLINK_COMM_0].send_gps_raw(tracker.gps); |
|
|
|
|
send_gps_raw(tracker.gps); |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case MSG_RADIO_IN: |
|
|
|
|
CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW); |
|
|
|
|
tracker.gcs[chan-MAVLINK_COMM_0].send_radio_in(0); |
|
|
|
|
send_radio_in(0); |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case MSG_RADIO_OUT: |
|
|
|
@ -209,22 +209,22 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
@@ -209,22 +209,22 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
|
|
|
|
|
|
|
|
|
|
case MSG_RAW_IMU1: |
|
|
|
|
CHECK_PAYLOAD_SIZE(RAW_IMU); |
|
|
|
|
tracker.gcs[chan-MAVLINK_COMM_0].send_raw_imu(tracker.ins, tracker.compass); |
|
|
|
|
send_raw_imu(tracker.ins, tracker.compass); |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case MSG_RAW_IMU2: |
|
|
|
|
CHECK_PAYLOAD_SIZE(SCALED_PRESSURE); |
|
|
|
|
tracker.gcs[chan-MAVLINK_COMM_0].send_scaled_pressure(tracker.barometer); |
|
|
|
|
send_scaled_pressure(tracker.barometer); |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case MSG_RAW_IMU3: |
|
|
|
|
CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS); |
|
|
|
|
tracker.gcs[chan-MAVLINK_COMM_0].send_sensor_offsets(tracker.ins, tracker.compass, tracker.barometer); |
|
|
|
|
send_sensor_offsets(tracker.ins, tracker.compass, tracker.barometer); |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case MSG_NEXT_PARAM: |
|
|
|
|
CHECK_PAYLOAD_SIZE(PARAM_VALUE); |
|
|
|
|
tracker.gcs[chan-MAVLINK_COMM_0].queued_param_send(); |
|
|
|
|
queued_param_send(); |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case MSG_NEXT_WAYPOINT: |
|
|
|
@ -238,7 +238,7 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
@@ -238,7 +238,7 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
|
|
|
|
|
|
|
|
|
|
case MSG_AHRS: |
|
|
|
|
CHECK_PAYLOAD_SIZE(AHRS); |
|
|
|
|
tracker.gcs[chan-MAVLINK_COMM_0].send_ahrs(tracker.ahrs); |
|
|
|
|
send_ahrs(tracker.ahrs); |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case MSG_SIMSTATE: |
|
|
|
@ -695,7 +695,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
@@ -695,7 +695,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|
|
|
|
|
|
|
|
|
case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: { |
|
|
|
|
if (is_equal(packet.param1,1.0f)) { |
|
|
|
|
tracker.gcs[chan-MAVLINK_COMM_0].send_autopilot_version(FIRMWARE_VERSION); |
|
|
|
|
send_autopilot_version(FIRMWARE_VERSION); |
|
|
|
|
result = MAV_RESULT_ACCEPTED; |
|
|
|
|
} |
|
|
|
|
break; |
|
|
|
@ -883,7 +883,7 @@ mission_failed:
@@ -883,7 +883,7 @@ mission_failed:
|
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST: |
|
|
|
|
tracker.gcs[chan-MAVLINK_COMM_0].send_autopilot_version(FIRMWARE_VERSION); |
|
|
|
|
send_autopilot_version(FIRMWARE_VERSION); |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
} // end switch
|
|
|
|
|