void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
{
switch (msg->msgid) {
// If we are currently operating as a proxy for a remote,
// alas we have to look inside each packet to see if its for us or for the remote
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
{
handle_request_data_stream(msg, true);
break;
}
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
{
handle_param_request_list(msg);
break;
}
case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
{
handle_param_request_read(msg);
break;
}
case MAVLINK_MSG_ID_PARAM_SET:
{
handle_param_set(msg, NULL);
break;
}
case MAVLINK_MSG_ID_HEARTBEAT:
break;
case MAVLINK_MSG_ID_COMMAND_LONG:
{
// decode
mavlink_command_long_t packet;
mavlink_msg_command_long_decode(msg, &packet);
uint8_t result = MAV_RESULT_UNSUPPORTED;
// do command
send_text_P(SEVERITY_LOW,PSTR("command received: "));
switch(packet.command) {
case MAV_CMD_PREFLIGHT_CALIBRATION:
{
if (is_equal(packet.param1,1.0f)) {
tracker.ins.init_gyro();
if (tracker.ins.gyro_calibrated_ok_all()) {
tracker.ahrs.reset_gyro_drift();
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
}
if (is_equal(packet.param3,1.0f)) {
tracker.init_barometer();
// zero the altitude difference on next baro update
tracker.nav_status.need_altitude_calibration = true;
}
if (is_equal(packet.param4,1.0f)) {
// Cant trim radio
} else if (is_equal(packet.param5,1.0f)) {
float trim_roll, trim_pitch;
AP_InertialSensor_UserInteract_MAVLink interact(this);
if(tracker.ins.calibrate_accel(&interact, trim_roll, trim_pitch)) {
// reset ahrs's trim to suggested values from calibration routine
tracker.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
}
} else if (is_equal(packet.param5,2.0f)) {
// accel trim
float trim_roll, trim_pitch;
if(tracker.ins.calibrate_trim(trim_roll, trim_pitch)) {
// reset ahrs's trim to suggested values from calibration routine
tracker.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
}
result = MAV_RESULT_ACCEPTED;
break;
}
case MAV_CMD_COMPONENT_ARM_DISARM:
if (packet.target_component == MAV_COMP_ID_SYSTEM_CONTROL) {
if (is_equal(packet.param1,1.0f)) {
tracker.arm_servos();
result = MAV_RESULT_ACCEPTED;
} else if (is_zero(packet.param1)) {
tracker.disarm_servos();
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_UNSUPPORTED;
}
} else {
result = MAV_RESULT_UNSUPPORTED;
}
break;
case MAV_CMD_DO_SET_MODE:
switch ((uint16_t)packet.param1) {
case MAV_MODE_MANUAL_ARMED:
case MAV_MODE_MANUAL_DISARMED:
tracker.set_mode(MANUAL);
result = MAV_RESULT_ACCEPTED;
break;
case MAV_MODE_AUTO_ARMED:
case MAV_MODE_AUTO_DISARMED:
tracker.set_mode(AUTO);
result = MAV_RESULT_ACCEPTED;
break;
default:
result = MAV_RESULT_UNSUPPORTED;
}
break;
case MAV_CMD_DO_SET_SERVO:
if (tracker.servo_test_set_servo(packet.param1, packet.param2)) {
result = MAV_RESULT_ACCEPTED;
}
break;
// mavproxy/mavutil sends this when auto command is entered
case MAV_CMD_MISSION_START:
tracker.set_mode(AUTO);
result = MAV_RESULT_ACCEPTED;
break;
case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
{
if (is_equal(packet.param1,1.0f) || is_equal(packet.param1,3.0f)) {
// when packet.param1 == 3 we reboot to hold in bootloader
hal.scheduler->reboot(is_equal(packet.param1,3.0f));
result = MAV_RESULT_ACCEPTED;
}
break;
}
case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: {
if (is_equal(packet.param1,1.0f)) {
tracker.gcs[chan-MAVLINK_COMM_0].send_autopilot_version();
result = MAV_RESULT_ACCEPTED;
}
break;
}
default:
break;
}
mavlink_msg_command_ack_send(
chan,
packet.command,
result);
break;
}
// When mavproxy 'wp sethome'
case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST:
{
// decode
mavlink_mission_write_partial_list_t packet;
mavlink_msg_mission_write_partial_list_decode(msg, &packet);
if (packet.start_index == 0)
{
// New home at wp index 0. Ask for it
waypoint_receiving = true;
waypoint_request_i = 0;
waypoint_request_last = 0;
send_message(MSG_NEXT_WAYPOINT);
waypoint_receiving = true;
}
break;
}
// XXX receive a WP from GCS and store in EEPROM if it is HOME
case MAVLINK_MSG_ID_MISSION_ITEM:
{
// decode
mavlink_mission_item_t packet;
uint8_t result = MAV_MISSION_ACCEPTED;
mavlink_msg_mission_item_decode(msg, &packet);
struct Location tell_command = {};
switch (packet.frame)
{
case MAV_FRAME_MISSION:
case MAV_FRAME_GLOBAL:
{
tell_command.lat = 1.0e7f*packet.x; // in as DD converted to * t7
tell_command.lng = 1.0e7f*packet.y; // in as DD converted to * t7
tell_command.alt = packet.z*1.0e2f; // in as m converted to cm
tell_command.options = 0; // absolute altitude
break;
}
#ifdef MAV_FRAME_LOCAL_NED
case MAV_FRAME_LOCAL_NED: // local (relative to home position)
{
tell_command.lat = 1.0e7f*ToDeg(packet.x/
(RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat;
tell_command.lng = 1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng;
tell_command.alt = -packet.z*1.0e2f;
tell_command.options = MASK_OPTIONS_RELATIVE_ALT;
break;
}
#endif
#ifdef MAV_FRAME_LOCAL
case MAV_FRAME_LOCAL: // local (relative to home position)
{
tell_command.lat = 1.0e7f*ToDeg(packet.x/
(RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat;
tell_command.lng = 1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng;
tell_command.alt = packet.z*1.0e2f;
tell_command.options = MASK_OPTIONS_RELATIVE_ALT;
break;
}
#endif
case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude
{
tell_command.lat = 1.0e7f * packet.x; // in as DD converted to * t7
tell_command.lng = 1.0e7f * packet.y; // in as DD converted to * t7
tell_command.alt = packet.z * 1.0e2f;
tell_command.options = MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!!
break;
}
default:
result = MAV_MISSION_UNSUPPORTED_FRAME;
break;
}
if (result != MAV_MISSION_ACCEPTED) goto mission_failed;
// Check if receiving waypoints (mission upload expected)
if (!waypoint_receiving) {
result = MAV_MISSION_ERROR;
goto mission_failed;
}
// check if this is the HOME wp
if (packet.seq == 0) {
tracker.set_home(tell_command); // New home in EEPROM
send_text_P(SEVERITY_LOW,PSTR("new HOME received"));
waypoint_receiving = false;
}
mission_failed:
// we are rejecting the mission/waypoint
mavlink_msg_mission_ack_send(
chan,
msg->sysid,
msg->compid,
result);
break;
}
case MAVLINK_MSG_ID_MANUAL_CONTROL:
{
mavlink_manual_control_t packet;
mavlink_msg_manual_control_decode(msg, &packet);
tracker.tracking_manual_control(packet);
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
// decode
mavlink_global_position_int_t packet;
mavlink_msg_global_position_int_decode(msg, &packet);
tracker.tracking_update_position(packet);
break;
}
case MAVLINK_MSG_ID_SCALED_PRESSURE:
{
// decode
mavlink_scaled_pressure_t packet;
mavlink_msg_scaled_pressure_decode(msg, &packet);
tracker.tracking_update_pressure(packet);
break;
}
case MAVLINK_MSG_ID_SET_MODE:
{
handle_set_mode(msg, FUNCTOR_BIND(&tracker, &Tracker::mavlink_set_mode, bool, uint8_t));
break;
}
#if HAL_CPU_CLASS > HAL_CPU_CLASS_16
case MAVLINK_MSG_ID_SERIAL_CONTROL:
handle_serial_control(msg, tracker.gps);
break;
case MAVLINK_MSG_ID_GPS_INJECT_DATA:
handle_gps_inject(msg, tracker.gps);
break;
#endif
case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST:
tracker.gcs[chan-MAVLINK_COMM_0].send_autopilot_version();
break;
} // end switch
} // end handle mavlink
// ------------------------------------------------------------------------------
// Read Messages
// ------------------------------------------------------------------------------
void
Autopilot_Interface::
read_messages()
{
bool success; // receive success flag
bool received_all = false; // receive only one message
Time_Stamps this_timestamps;
// Blocking wait for new data
while ( !received_all and !time_to_exit )
{
// ----------------------------------------------------------------------
// READ MESSAGE
// ----------------------------------------------------------------------
mavlink_message_t message;
success = serial_port->read_message(message);
// ----------------------------------------------------------------------
// HANDLE MESSAGE
// ----------------------------------------------------------------------
if( success )
{
// Store message sysid and compid.
// Note this doesn't handle multiple message sources.
current_messages.sysid = message.sysid;
current_messages.compid = message.compid;
// Handle Message ID
switch (message.msgid)
{
case MAVLINK_MSG_ID_HEARTBEAT:
{
printf("MAVLINK_MSG_ID_HEARTBEAT\n");
mavlink_msg_heartbeat_decode(&message, &(current_messages.heartbeat));
current_messages.time_stamps.heartbeat = get_time_usec();
this_timestamps.heartbeat = current_messages.time_stamps.heartbeat;
break;
}
case MAVLINK_MSG_ID_SYS_STATUS:
{
printf("MAVLINK_MSG_ID_SYS_STATUS\n");
mavlink_msg_sys_status_decode(&message, &(current_messages.sys_status));
current_messages.time_stamps.sys_status = get_time_usec();
this_timestamps.sys_status = current_messages.time_stamps.sys_status;
break;
}
case MAVLINK_MSG_ID_BATTERY_STATUS:
{
//printf("MAVLINK_MSG_ID_BATTERY_STATUS\n");
mavlink_msg_battery_status_decode(&message, &(current_messages.battery_status));
current_messages.time_stamps.battery_status = get_time_usec();
this_timestamps.battery_status = current_messages.time_stamps.battery_status;
break;
}
case MAVLINK_MSG_ID_RADIO_STATUS:
{
//printf("MAVLINK_MSG_ID_RADIO_STATUS\n");
mavlink_msg_radio_status_decode(&message, &(current_messages.radio_status));
current_messages.time_stamps.radio_status = get_time_usec();
this_timestamps.radio_status = current_messages.time_stamps.radio_status;
break;
}
case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
{
//printf("MAVLINK_MSG_ID_LOCAL_POSITION_NED\n");
mavlink_msg_local_position_ned_decode(&message, &(current_messages.local_position_ned));
current_messages.time_stamps.local_position_ned = get_time_usec();
this_timestamps.local_position_ned = current_messages.time_stamps.local_position_ned;
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
//printf("MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n");
mavlink_msg_global_position_int_decode(&message, &(current_messages.global_position_int));
current_messages.time_stamps.global_position_int = get_time_usec();
this_timestamps.global_position_int = current_messages.time_stamps.global_position_int;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
{
//printf("MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n");
mavlink_msg_position_target_local_ned_decode(&message, &(current_messages.position_target_local_ned));
current_messages.time_stamps.position_target_local_ned = get_time_usec();
this_timestamps.position_target_local_ned = current_messages.time_stamps.position_target_local_ned;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
{
//printf("MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n");
mavlink_msg_position_target_global_int_decode(&message, &(current_messages.position_target_global_int));
current_messages.time_stamps.position_target_global_int = get_time_usec();
this_timestamps.position_target_global_int = current_messages.time_stamps.position_target_global_int;
break;
}
case MAVLINK_MSG_ID_HIGHRES_IMU:
{
//printf("MAVLINK_MSG_ID_HIGHRES_IMU\n");
mavlink_msg_highres_imu_decode(&message, &(current_messages.highres_imu));
current_messages.time_stamps.highres_imu = get_time_usec();
this_timestamps.highres_imu = current_messages.time_stamps.highres_imu;
break;
}
case MAVLINK_MSG_ID_ATTITUDE:
{
//printf("MAVLINK_MSG_ID_ATTITUDE\n");
mavlink_msg_attitude_decode(&message, &(current_messages.attitude));
current_messages.time_stamps.attitude = get_time_usec();
this_timestamps.attitude = current_messages.time_stamps.attitude;
break;
}
case MAVLINK_MSG_ID_GPS_RAW_INT:
{
//printf("MAVLINK_MSG_ID_GPS_RAW_INT\n");
mavlink_msg_gps_raw_int_decode(&message, &(current_messages));
current_messages.time_stamps.gps_raw= get_time_usec();
this_timestamps.gps_raw= current_messages.time_stamps.gps_raw;
break;
}
default:
{
// printf("Warning, did not handle message id %i\n",message.msgid);
break;
}
} // end: switch msgid
} // end: if read message
// Check for receipt of all items
received_all =
this_timestamps.heartbeat &&
// this_timestamps.battery_status &&
// this_timestamps.radio_status &&
this_timestamps.local_position_ned &&
this_timestamps.global_position_int &&
// this_timestamps.position_target_local_ned &&
// this_timestamps.position_target_global_int &&
// this_timestamps.highres_imu &&
this_timestamps.attitude &&
this_timestamps.sys_status
;
// give the write thread time to use the port
if ( writing_status > false ) {
usleep(100); // look for components of batches at 10kHz
}
} // end: while not received all
return;
}
// handle mavlink DISTANCE_SENSOR messages
void AP_Follow::handle_msg(const mavlink_message_t &msg)
{
// exit immediately if not enabled
if (!_enabled) {
return;
}
// skip our own messages
if (msg.sysid == mavlink_system.sysid) {
return;
}
// skip message if not from our target
if ((_sysid_to_follow != 0) && (msg.sysid != _sysid_to_follow)) {
if (_sysid == 0) {
// maybe timeout who we were following...
if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_SYSID_TIMEOUT_MS)) {
_sysid_to_follow = 0;
}
}
return;
}
// decode global-position-int message
if (msg.msgid == MAVLINK_MSG_ID_GLOBAL_POSITION_INT) {
const uint32_t now = AP_HAL::millis();
// get estimated location and velocity (for logging)
Location loc_estimate{};
Vector3f vel_estimate;
UNUSED_RESULT(get_target_location_and_velocity(loc_estimate, vel_estimate));
// decode message
mavlink_global_position_int_t packet;
mavlink_msg_global_position_int_decode(&msg, &packet);
// ignore message if lat and lon are (exactly) zero
if ((packet.lat == 0 && packet.lon == 0)) {
return;
}
_target_location.lat = packet.lat;
_target_location.lng = packet.lon;
_target_location.alt = packet.alt / 10; // convert millimeters to cm
_target_velocity_ned.x = packet.vx * 0.01f; // velocity north
_target_velocity_ned.y = packet.vy * 0.01f; // velocity east
_target_velocity_ned.z = packet.vz * 0.01f; // velocity down
_last_location_update_ms = now;
if (packet.hdg <= 36000) { // heading (UINT16_MAX if unknown)
_target_heading = packet.hdg * 0.01f; // convert centi-degrees to degrees
_last_heading_update_ms = now;
}
// initialise _sysid if zero to sender's id
if (_sysid_to_follow == 0) {
_sysid_to_follow = msg.sysid;
}
if ((AP_HAL::millis() - _last_location_sent_to_gcs > AP_GCS_INTERVAL_MS)) {
gcs().send_text(MAV_SEVERITY_INFO, "Foll: %u %ld %ld %4.2f\n",
(unsigned)_sysid_to_follow,
(long)_target_location.lat,
(long)_target_location.lng,
(double)(_target_location.alt * 0.01f)); // cm to m
}
// log lead's estimated vs reported position
DataFlash_Class::instance()->Log_Write("FOLL",
"TimeUS,Lat,Lon,Alt,VelX,VelY,VelZ,LatE,LonE,AltE", // labels
"sDUmnnnDUm", // units
"F--B000--B", // mults
"QLLifffLLi", // fmt
AP_HAL::micros64(),
_target_location.lat,
_target_location.lng,
_target_location.alt,
(double)_target_velocity_ned.x,
(double)_target_velocity_ned.y,
(double)_target_velocity_ned.z,
loc_estimate.lat,
loc_estimate.lng,
loc_estimate.alt
);
}
}
//Function that reads out the port and stores it into a buffer of type mavlink_message_t
int read_message(mavlink_message_t *message)
{
// Struct of status of the reading/writing process. Defined in mavlink_types.h. For development/debugging purposes
mavlink_status_t status;
// If the data read out from the port is a whole mavlink message package msgReceived will be set to 1
uint8_t msgReceived = 0;
// Initialize a buffer of 64 byte (array of 64 uint8_t). Bytes read out from the Serial Port are stored into cp
uint8_t cp[1000];
// This function locks the port during read and reads out the port (nonblocking, up to sizeof(cp) bytes).
// Result is the number of bytes read
int result = _read_port(cp);
// Counter of the for loop needs to be declared outside of for loop. for(int i = 0; i < result; i++) results in a compiler error
int i;
// If _read_port(cp) reads out something from the serial port the bytes will be processed in the following
if (result > 0)
{
// mavlink_parse_char parses only chars. Therefore each read byte needs to be parsed individually
for(i = 0; i < result; i++)
{
// Debug purposes
if(debug == 1)
{
printf("cp: %02x \n ",cp[i]);
}
// Parsing one byte on Channel MAVLINK_COMM_1 (1) and storing the current status into mavlink_status_t status
// The parsed bytes are stored in an internal buffer on channel MAVLINK_COMM_1
// If the last byte arrived completes the message the message is copied from the internal buffer to mavlink_message_t message
// msgReceived will be set to 1 if the parsed byte completes the message. 0 otherwise
msgReceived = mavlink_parse_char(MAVLINK_COMM_1, cp[i], message, &status);
// If a mavlink message is received (all the bytes needed for a message processed by mavlink_parse_char()) handle the message by it's ID
if(msgReceived)
{
//Analysis values
all_counter += msgReceived;
/*printf("Allcounter %i \n", *all_counter);*/
werte = message->msgid;
liste[counter] = werte;
counter++;
/*printf("MSG RECEIVED and with msgid %i \n", *werte);*/
// Handle Message ID. Switching to the case depending on the msg.id from the read msg.
// First the message gets decoded by the function in the corresponding header in C_library/common/foobar.h. It is then stored (memcpy) in current_messages.msg_type
switch (message->msgid)
{
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
// Decoding the message (consisting of a header and a payload with the actual content) to the corresponding Mavlink Message
mavlink_msg_global_position_int_decode(message,&(messages.global_position_int));
// Starting a timer (analysis purposes)
if (all_counter == 1){
t_px_start = messages.global_position_int.time_boot_ms/1e3;
t_last = t_px_start;
}
t_px_stop = messages.global_position_int.time_boot_ms/1e3;
printf("Sample: %f \n", t_px_stop - t_last);
t_last = t_px_stop;
/*printf("TIME: \f \n", messages.global_position_int.time_boot_ms/1);*/
break;
}
default:
{
break;
}
} // end: switch msgid
// Debug
if(status.buffer_overrun == 1 | status.parse_error == 1 | status.packet_rx_drop_count == 1)
{
printf("Buffer overruns: %i, Parse Errors: %i , Packets dropped: %i \n", status.buffer_overrun, status.parse_error, status.packet_rx_drop_count);
}
// check for dropped packets
if ( (lastStatus.packet_rx_drop_count != status.packet_rx_drop_count) && debug )
{
printf("ERROR: DROPPED %d PACKETS\n", status.packet_rx_drop_count);
packets_dropped++;
}
else if (lastStatus.packet_rx_drop_count != status.packet_rx_drop_count)
{
packets_dropped++;
}
else
{
}
lastStatus = status;
}
}
}
// Couldn't read from port (nothing on the serial port, if reading frequency is higher than transmitting frequency)
else
{
printf("COULDNT READ FROM PORT");
}
// --------------------------------------------------------------------------
// DEBUGGING REPORTS
// --------------------------------------------------------------------------
if(msgReceived && debug)
{
// Printing the message read (in hexadecimal)
fprintf(stderr,"Received serial data: ");
unsigned int i;
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
// check message is write length
unsigned int messageLength = mavlink_msg_to_send_buffer(buffer, message);
// message length error
if (messageLength > MAVLINK_MAX_PACKET_LEN)
{
fprintf(stderr, "\nFATAL ERROR: MESSAGE LENGTH IS LARGER THAN BUFFER SIZE\n");
}
// print out the buffer
else
{
for (i=0; i<messageLength; i++)
{
unsigned char v=buffer[i];
fprintf(stderr,"%02x ", v);
}
fprintf(stderr,"\n");
}
}
// Some analysis variables for run.c
// Done!
return msgReceived;
}
void MAVLink_Message_Handler::handle_message(uint64_t timestamp, mavlink_message_t &msg)
{
// ::fprintf(stderr, "msg.msgid=%u\n", msg.msgid);
switch(msg.msgid) {
case MAVLINK_MSG_ID_AHRS2: {
mavlink_ahrs2_t decoded;
mavlink_msg_ahrs2_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_ATTITUDE: {
mavlink_attitude_t decoded;
mavlink_msg_attitude_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_EKF_STATUS_REPORT: {
mavlink_ekf_status_report_t decoded;
mavlink_msg_ekf_status_report_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
mavlink_global_position_int_t decoded;
mavlink_msg_global_position_int_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_GPS_RAW_INT: {
mavlink_gps_raw_int_t decoded;
mavlink_msg_gps_raw_int_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_HEARTBEAT: {
mavlink_heartbeat_t decoded;
mavlink_msg_heartbeat_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_MOUNT_STATUS: {
mavlink_mount_status_t decoded;
mavlink_msg_mount_status_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: {
mavlink_nav_controller_output_t decoded;
mavlink_msg_nav_controller_output_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_PARAM_VALUE: {
mavlink_param_value_t decoded;
mavlink_msg_param_value_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK: {
mavlink_remote_log_data_block_t decoded;
mavlink_msg_remote_log_data_block_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_SCALED_PRESSURE: {
mavlink_scaled_pressure_t decoded;
mavlink_msg_scaled_pressure_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_SCALED_PRESSURE2: {
mavlink_scaled_pressure2_t decoded;
mavlink_msg_scaled_pressure2_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: {
mavlink_servo_output_raw_t decoded;
mavlink_msg_servo_output_raw_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_STATUSTEXT: {
mavlink_statustext_t decoded;
mavlink_msg_statustext_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_SYS_STATUS: {
mavlink_sys_status_t decoded;
mavlink_msg_sys_status_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_SYSTEM_TIME: {
mavlink_system_time_t decoded;
mavlink_msg_system_time_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_VFR_HUD: {
mavlink_vfr_hud_t decoded;
mavlink_msg_vfr_hud_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
}
}
// ------------------------------------------------------------------------------
// Read Messages
// ------------------------------------------------------------------------------
void Autopilot_Interface::read_messages() {
bool success; // receive success flag
bool received_all = false; // receive only one message
Time_Stamps this_timestamps;
// Blocking wait for new data
while (!received_all and !time_to_exit) {
// ----------------------------------------------------------------------
// READ MESSAGE
// ----------------------------------------------------------------------
mavlink_message_t message;
success = serial_port->read_message(message);
// ----------------------------------------------------------------------
// HANDLE MESSAGE
// ----------------------------------------------------------------------
if (success) {
// Store message sysid and compid.
// Note this doesn't handle multiple message sources.
current_messages.sysid = message.sysid;
current_messages.compid = message.compid;
// Handle Message ID
switch (message.msgid) {
case MAVLINK_MSG_ID_HEARTBEAT: {
printf("MAVLINK_MSG_ID_HEARTBEAT\n");
mavlink_msg_heartbeat_decode(&message, &(current_messages.heartbeat));
current_messages.time_stamps.heartbeat = get_time_usec();
this_timestamps.heartbeat = current_messages.time_stamps.heartbeat;
break;
}
case MAVLINK_MSG_ID_SYS_STATUS: {
printf("MAVLINK_MSG_ID_SYS_STATUS\n");
mavlink_msg_sys_status_decode(&message,
&(current_messages.sys_status));
current_messages.time_stamps.sys_status = get_time_usec();
this_timestamps.sys_status = current_messages.time_stamps.sys_status;
printf("Battery life remaining: %i\n",
current_messages.sys_status.battery_remaining);
break;
}
// this is not being received
case MAVLINK_MSG_ID_BATTERY_STATUS: {
printf("MAVLINK_MSG_ID_BATTERY_STATUS\n");
mavlink_msg_battery_status_decode(&message,
&(current_messages.battery_status));
current_messages.time_stamps.battery_status = get_time_usec();
this_timestamps.battery_status =
current_messages.time_stamps.battery_status;
printf("Battery life remaining: %i\n",
current_messages.battery_status.battery_remaining);
break;
}
case MAVLINK_MSG_ID_RADIO_STATUS: {
printf("MAVLINK_MSG_ID_RADIO_STATUS\n");
mavlink_msg_radio_status_decode(&message,
&(current_messages.radio_status));
current_messages.time_stamps.radio_status = get_time_usec();
this_timestamps.radio_status =
current_messages.time_stamps.radio_status;
break;
}
case MAVLINK_MSG_ID_LOCAL_POSITION_NED: {
printf("MAVLINK_MSG_ID_LOCAL_POSITION_NED\n");
mavlink_msg_local_position_ned_decode(
&message, &(current_messages.local_position_ned));
printf(" pos (GPS): %f %f %f (m)\n",
current_messages.local_position_ned.x,
current_messages.local_position_ned.y,
current_messages.local_position_ned.z);
current_messages.time_stamps.local_position_ned = get_time_usec();
this_timestamps.local_position_ned =
current_messages.time_stamps.local_position_ned;
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
printf("MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n");
mavlink_msg_global_position_int_decode(
&message, &(current_messages.global_position_int));
current_messages.time_stamps.global_position_int = get_time_usec();
this_timestamps.global_position_int =
current_messages.time_stamps.global_position_int;
mavlink_global_position_int_t gpos =
current_messages.global_position_int;
printf(" pos (GPS): %f %f %f (m)\n", gpos.lat, gpos.lon,
gpos.alt);
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED: {
printf("MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n");
mavlink_msg_position_target_local_ned_decode(
&message, &(current_messages.position_target_local_ned));
current_messages.time_stamps.position_target_local_ned =
get_time_usec();
this_timestamps.position_target_local_ned =
current_messages.time_stamps.position_target_local_ned;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT: {
printf("MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n");
mavlink_msg_position_target_global_int_decode(
&message, &(current_messages.position_target_global_int));
current_messages.time_stamps.position_target_global_int =
get_time_usec();
this_timestamps.position_target_global_int =
current_messages.time_stamps.position_target_global_int;
break;
}
case MAVLINK_MSG_ID_HIGHRES_IMU: {
printf("MAVLINK_MSG_ID_HIGHRES_IMU\n");
mavlink_msg_highres_imu_decode(&message,
&(current_messages.highres_imu));
current_messages.time_stamps.highres_imu = get_time_usec();
this_timestamps.highres_imu =
current_messages.time_stamps.highres_imu;
mavlink_highres_imu_t imu = current_messages.highres_imu;
printf(" ap time: %llu \n", imu.time_usec);
printf(" acc (NED): % f % f % f (m/s^2)\n", imu.xacc, imu.yacc,
imu.zacc);
printf(" gyro (NED): % f % f % f (rad/s)\n", imu.xgyro, imu.ygyro,
imu.zgyro);
printf(" mag (NED): % f % f % f (Ga)\n", imu.xmag, imu.ymag,
imu.zmag);
printf(" baro: %f (mBar) \n", imu.abs_pressure);
printf(" altitude: %f (m) \n", imu.pressure_alt);
printf(" temperature: %f C \n", imu.temperature);
break;
}
case MAVLINK_MSG_ID_ATTITUDE: {
printf("### MAVLINK_MSG_ID_ATTITUDE\n");
mavlink_msg_attitude_decode(&message, &(current_messages.attitude));
current_messages.time_stamps.attitude = get_time_usec();
this_timestamps.attitude = current_messages.time_stamps.attitude;
mavlink_attitude_t att = current_messages.attitude;
printf("### roll: %f pitch: %f yaw: %f\n", att.roll, att.pitch,
att.yaw);
break;
}
default: {
// printf("Warning, did not handle message id %i\n",message.msgid);
break;
}
} // end: switch msgid
} // end: if read message
// Check for receipt of all items
received_all =
this_timestamps.heartbeat &&
// this_timestamps.battery_status &&
// this_timestamps.radio_status &&
// this_timestamps.local_position_ned &&
// this_timestamps.global_position_int &&
// this_timestamps.position_target_local_ned
//&&
// this_timestamps.position_target_global_int
//&&
// this_timestamps.highres_imu &&
// this_timestamps.attitude &&
this_timestamps.sys_status;
// give the write thread time to use the port
if (writing_status > false) {
usleep(100); // look for components of batches at 10kHz
}
} // end: while not received all
return;
}
