void MavLinkInterpreter::ProcessGpsRaw() const { _m->FixType = mavlink_msg_gps_raw_int_get_fix_type(&_msg); // 0 = No GPS, 1 =No Fix, 2 = 2D Fix, 3 = 3D Fix, 4 = DGPS, 5 = RTK _m->SatVisible = mavlink_msg_gps_raw_int_get_satellites_visible(&_msg); // numbers of visible satelites _m->GpsStatus = (_m->SatVisible * 10) + _m->FixType; _m->GpsHdop = mavlink_msg_gps_raw_int_get_eph(&_msg); // Max 8 bit if (_m->GpsHdop == 0 || _m->GpsHdop > 255) { _m->GpsHdop = 255; } if (_m->FixType >= 3) { _m->Latitude = mavlink_msg_gps_raw_int_get_lat(&_msg); _m->Longitude = mavlink_msg_gps_raw_int_get_lon(&_msg); _m->GpsAltitude = mavlink_msg_gps_raw_int_get_alt(&_msg); // 1m = 1000 _m->GpsSpeed = mavlink_msg_gps_raw_int_get_vel(&_msg); // 100 = 1m/s _m->Cog = mavlink_msg_gps_raw_int_get_cog(&_msg) / 100; } else { _m->GpsSpeed = 0; } #ifdef DEBUG_APM_GPS_RAW debugSerial.printf("%d\tMAVLINK_MSG_ID_GPS_RAW_INT: fixtype: %d, visiblesats: %d, _status: %d, gpsspeed: %d, hdop: %d, alt: %d\r\n", millis(), _m->FixType, _m->SatVisible, _m->GpsStatus, _m->GpsSpeed, _m->GpsHdop, _m->GpsAltitude); #endif }
static void mav_callback(mavlink_message_t *msg, void *d) { struct widget *w = d; struct widget_priv *priv = w->priv; priv->gps_lat = mavlink_msg_gps_raw_int_get_lat(msg) / 10000000.0; priv->gps_lon = mavlink_msg_gps_raw_int_get_lon(msg) / 10000000.0; priv->gps_fix_type = mavlink_msg_gps_raw_int_get_fix_type(msg); priv->gps_nrsats = mavlink_msg_gps_raw_int_get_satellites_visible(msg); priv->gps_eph = (float) mavlink_msg_gps_raw_int_get_eph(msg) / 100.0; schedule_widget(w); }
static void store_mavdata(mavlink_message_t *msg, mavlink_status_t *status) { switch (msg->msgid) { case MAVLINK_MSG_ID_VFR_HUD: priv.altitude = (unsigned int) mavlink_msg_vfr_hud_get_alt(msg); priv.heading = mavlink_msg_vfr_hud_get_heading(msg); break; case MAVLINK_MSG_ID_GPS_RAW_INT: priv.uav_coord.lat = DEG2RAD(mavlink_msg_gps_raw_int_get_lat(msg) / 10000000.0); priv.uav_coord.lon = DEG2RAD(mavlink_msg_gps_raw_int_get_lon(msg) / 10000000.0); priv.gps_fix_type = mavlink_msg_gps_raw_int_get_fix_type(msg); break; default: break; } }
static void sol_mavlink_position_handler(struct sol_mavlink *mavlink, mavlink_message_t *msg) { int32_t lat, longi; float alt; struct sol_mavlink_position *pos = &mavlink->curr_position; lat = mavlink_msg_gps_raw_int_get_lat(msg); longi = mavlink_msg_gps_raw_int_get_lon(msg); alt = mavlink_msg_gps_raw_int_get_alt(msg); alt = (alt - mavlink->home_position.altitude) / 1000.0f; if (lat != pos->latitude || longi != pos->longitude || alt != pos->altitude) { pos->latitude = lat / 1.0e7f; pos->longitude = longi / 1.0e7f; pos->altitude = alt; mavlink->status |= SOL_MAVLINK_STATUS_GPS_SETUP; if (CHECK_HANDLER(mavlink, position_changed)) HANDLERS(mavlink)->position_changed((void *)mavlink->data, mavlink); } }
int main(int argc, char **argv){ //open com port char *portname = "/dev/ttyACM0"; char *baud = "57600"; int c; while((c = getopt(argc, argv, "p:b:")) != -1) switch(c){ case 'p': portname = optarg; break; case 'b': baud = optarg; break; default: printf("error in command line arguments\n"); exit(1); break; } printf("serial port: %s, baud rate: %s\n", portname, baud); int fd = open (portname, O_RDWR | O_NOCTTY | O_SYNC); //terminate if unable to open com port if (fd < 0) { error_message ("error %d opening %s: %s\n", errno, portname, strerror (errno)); return; } //configure com port connection if(baud == "57600") set_interface_attribs (fd, B57600, 0); // set speed to 57600 bps, 8n1 (no parity) else if (baud == "115200") set_interface_attribs (fd, B115200, 0); set_blocking (fd, 0); // set no blocking printf("serial connection established\n"); mavlink_message_t msg; uint8_t buf[MAVLINK_MAX_PACKET_LEN]; //buffer for sending and receiving messages int recsize = 0; //size of received message int i = 0; //loop variable while(1){ //send heartbeat to ardupilot mavlink_msg_heartbeat_pack(100, 200, &msg, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC, MAV_MODE_PREFLIGHT, 0, MAV_STATE_STANDBY); uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); write(fd, buf, len); //printf("quadrotor: %i, mavautopilot: %i, mavmode: %i, mavstate: %i\n", MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC, MAV_MODE_PREFLIGHT, MAV_STATE_STANDBY); //printf("LENGTH: %02x ", len); // int i; // for(i=0; i<len; i++) // { // printf("heartbeat: %02x ", (unsigned char) buf[i]); // } //clear buffer memset(buf, 0, MAVLINK_MAX_PACKET_LEN); //read from serial recsize = read(fd, buf, sizeof buf); //interpret message if the size of received data is greater than 0 if(recsize > 0){ mavlink_message_t msg; mavlink_status_t status; #ifdef DEBUG printf("Bytes Received: %d\n Datagram: ", (int)recsize); #endif for(i = 0; i < recsize; i++){ #ifdef DEBUG if(buf[i] == 254) printf("\n"); printf("%02x ", (unsigned char) buf[i]); #endif //use mavlink function to parse message if(mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status)){ #ifdef DEBUG printf("\nReceived packet: SYS: %d, COMP:%d, LEN:%d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid); #endif //extract data depending on the id number of the messages switch(msg.msgid){ case MAVLINK_MSG_ID_GPS_RAW_INT: printf("raw lat: %d, raw lon: %d\n", mavlink_msg_gps_raw_int_get_lat(&msg), mavlink_msg_gps_raw_int_get_lon(&msg)); break; case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: printf("lat: %d, lon: %d, alt: %d compass: %d\n", mavlink_msg_global_position_int_get_lat(&msg), mavlink_msg_global_position_int_get_lon(&msg), mavlink_msg_global_position_int_get_alt(&msg), mavlink_msg_global_position_int_get_hdg(&msg)); break; //add more cases here for other messages (see https://pixhawk.ethz.ch/mavlink/#DATA_STREAM and header(.h) files in mavlink/include/common/) } } } printf("\n\n"); } memset(buf, 0, MAVLINK_MAX_PACKET_LEN); sleep(1); } }
Json::Value getMavlinkParams(mavlink_message_t msg) { Json::Value content; switch (msg.msgid) { case 0: { //HEARTBEAT content["type"] = mavlink_msg_heartbeat_get_type(&msg); content["autopilot"] = mavlink_msg_heartbeat_get_autopilot(&msg); content["base_mode"] = mavlink_msg_heartbeat_get_base_mode(&msg); content["custom_mode"] = mavlink_msg_heartbeat_get_custom_mode(&msg); content["system_status"] = mavlink_msg_heartbeat_get_system_status(&msg); content["mavlink_version"] = mavlink_msg_heartbeat_get_mavlink_version(&msg); break; } case 1: { //SYS_STATUS content["onboard_control_sensors_present"] = mavlink_msg_sys_status_get_onboard_control_sensors_present(&msg); content["onboard_control_sensors_enabled"] = mavlink_msg_sys_status_get_onboard_control_sensors_enabled(&msg); content["onboard_control_sensors_health"] = mavlink_msg_sys_status_get_onboard_control_sensors_health(&msg); content["load"] = mavlink_msg_sys_status_get_load(&msg); content["voltage_battery"] = mavlink_msg_sys_status_get_voltage_battery(&msg); content["current_battery"] = mavlink_msg_sys_status_get_current_battery(&msg); content["battery_remaining"] = mavlink_msg_sys_status_get_battery_remaining(&msg); content["drop_rate_comm"] = mavlink_msg_sys_status_get_drop_rate_comm(&msg); content["errors_comm"] = mavlink_msg_sys_status_get_errors_comm(&msg); content["errors_count1"] = mavlink_msg_sys_status_get_errors_count1(&msg); content["errors_count2"] = mavlink_msg_sys_status_get_errors_count2(&msg); content["errors_count3"] = mavlink_msg_sys_status_get_errors_count3(&msg); content["errors_count4"] = mavlink_msg_sys_status_get_errors_count4(&msg); break; } case 2: { //SYSTEM_TIME content["time_unix_usec"] = mavlink_msg_system_time_get_time_unix_usec(&msg); content["time_boot_ms"] = mavlink_msg_system_time_get_time_boot_ms(&msg); break; } case 4: { //PING content["time_usec"] = mavlink_msg_ping_get_time_usec(&msg); content["seq"] = mavlink_msg_ping_get_seq(&msg); content["target_system"] = mavlink_msg_ping_get_target_system(&msg); content["target_component"] = mavlink_msg_ping_get_target_component(&msg); break; } case 5: { //CHANGE_OPERATOR_CONTROL content["target_system"] = mavlink_msg_change_operator_control_get_target_system(&msg); content["control_request"] = mavlink_msg_change_operator_control_get_control_request(&msg); content["version"] = mavlink_msg_change_operator_control_get_version(&msg); char arr_passkey[25]; mavlink_msg_change_operator_control_get_passkey(&msg, arr_passkey); content["passkey"] = std::string(arr_passkey, 25); break; } case 6: { //CHANGE_OPERATOR_CONTROL_ACK content["gcs_system_id"] = mavlink_msg_change_operator_control_ack_get_gcs_system_id(&msg); content["control_request"] = mavlink_msg_change_operator_control_ack_get_control_request(&msg); content["ack"] = mavlink_msg_change_operator_control_ack_get_ack(&msg); break; } case 7: { //AUTH_KEY char arr_key[32]; mavlink_msg_auth_key_get_key(&msg, arr_key); content["key"] = std::string(arr_key, 32); break; } case 11: { //SET_MODE content["target_system"] = mavlink_msg_set_mode_get_target_system(&msg); content["base_mode"] = mavlink_msg_set_mode_get_base_mode(&msg); content["custom_mode"] = mavlink_msg_set_mode_get_custom_mode(&msg); break; } case 20: { //PARAM_REQUEST_READ content["target_system"] = mavlink_msg_param_request_read_get_target_system(&msg); content["target_component"] = mavlink_msg_param_request_read_get_target_component(&msg); char arr_param_id[16]; mavlink_msg_param_request_read_get_param_id(&msg, arr_param_id); content["param_id"] = std::string(arr_param_id, 16); content["param_index"] = mavlink_msg_param_request_read_get_param_index(&msg); break; } case 21: { //PARAM_REQUEST_LIST content["target_system"] = mavlink_msg_param_request_list_get_target_system(&msg); content["target_component"] = mavlink_msg_param_request_list_get_target_component(&msg); break; } case 22: { //PARAM_VALUE char arr_param_id[16]; mavlink_msg_param_value_get_param_id(&msg, arr_param_id); content["param_id"] = std::string(arr_param_id, 16); content["param_value"] = mavlink_msg_param_value_get_param_value(&msg); content["param_type"] = mavlink_msg_param_value_get_param_type(&msg); content["param_count"] = mavlink_msg_param_value_get_param_count(&msg); content["param_index"] = mavlink_msg_param_value_get_param_index(&msg); break; } case 23: { //PARAM_SET content["target_system"] = mavlink_msg_param_set_get_target_system(&msg); content["target_component"] = mavlink_msg_param_set_get_target_component(&msg); char arr_param_id[16]; mavlink_msg_param_set_get_param_id(&msg, arr_param_id); content["param_id"] = std::string(arr_param_id, 16); content["param_value"] = mavlink_msg_param_set_get_param_value(&msg); content["param_type"] = mavlink_msg_param_set_get_param_type(&msg); break; } case 24: { //GPS_RAW_INT content["time_usec"] = mavlink_msg_gps_raw_int_get_time_usec(&msg); content["fix_type"] = mavlink_msg_gps_raw_int_get_fix_type(&msg); content["lat"] = mavlink_msg_gps_raw_int_get_lat(&msg); content["lon"] = mavlink_msg_gps_raw_int_get_lon(&msg); content["alt"] = mavlink_msg_gps_raw_int_get_alt(&msg); content["eph"] = mavlink_msg_gps_raw_int_get_eph(&msg); content["epv"] = mavlink_msg_gps_raw_int_get_epv(&msg); content["vel"] = mavlink_msg_gps_raw_int_get_vel(&msg); content["cog"] = mavlink_msg_gps_raw_int_get_cog(&msg); content["satellites_visible"] = mavlink_msg_gps_raw_int_get_satellites_visible(&msg); break; } case 25: { //GPS_STATUS content["satellites_visible"] = mavlink_msg_gps_status_get_satellites_visible(&msg); uint8_t arr_satellite_prn[20]; mavlink_msg_gps_status_get_satellite_prn(&msg, arr_satellite_prn); content["satellite_prn"] = Json::Value(Json::arrayValue); for(uint8_t i : arr_satellite_prn) { content["satellite_prn"].append(i); } uint8_t arr_satellite_used[20]; mavlink_msg_gps_status_get_satellite_used(&msg, arr_satellite_used); content["satellite_used"] = Json::Value(Json::arrayValue); for(uint8_t i : arr_satellite_used) { content["satellite_used"].append(i); } uint8_t arr_satellite_elevation[20]; mavlink_msg_gps_status_get_satellite_elevation(&msg, arr_satellite_elevation); content["satellite_elevation"] = Json::Value(Json::arrayValue); for(uint8_t i : arr_satellite_elevation) { content["satellite_elevation"].append(i); } uint8_t arr_satellite_azimuth[20]; mavlink_msg_gps_status_get_satellite_azimuth(&msg, arr_satellite_azimuth); content["satellite_azimuth"] = Json::Value(Json::arrayValue); for(uint8_t i : arr_satellite_azimuth) { content["satellite_azimuth"].append(i); } uint8_t arr_satellite_snr[20]; mavlink_msg_gps_status_get_satellite_snr(&msg, arr_satellite_snr); content["satellite_snr"] = Json::Value(Json::arrayValue); for(uint8_t i : arr_satellite_snr) { content["satellite_snr"].append(i); } break; } case 26: { //SCALED_IMU content["time_boot_ms"] = mavlink_msg_scaled_imu_get_time_boot_ms(&msg); content["xacc"] = mavlink_msg_scaled_imu_get_xacc(&msg); content["yacc"] = mavlink_msg_scaled_imu_get_yacc(&msg); content["zacc"] = mavlink_msg_scaled_imu_get_zacc(&msg); content["xgyro"] = mavlink_msg_scaled_imu_get_xgyro(&msg); content["ygyro"] = mavlink_msg_scaled_imu_get_ygyro(&msg); content["zgyro"] = mavlink_msg_scaled_imu_get_zgyro(&msg); content["xmag"] = mavlink_msg_scaled_imu_get_xmag(&msg); content["ymag"] = mavlink_msg_scaled_imu_get_ymag(&msg); content["zmag"] = mavlink_msg_scaled_imu_get_zmag(&msg); break; } case 27: { //RAW_IMU content["time_usec"] = mavlink_msg_raw_imu_get_time_usec(&msg); content["xacc"] = mavlink_msg_raw_imu_get_xacc(&msg); content["yacc"] = mavlink_msg_raw_imu_get_yacc(&msg); content["zacc"] = mavlink_msg_raw_imu_get_zacc(&msg); content["xgyro"] = mavlink_msg_raw_imu_get_xgyro(&msg); content["ygyro"] = mavlink_msg_raw_imu_get_ygyro(&msg); content["zgyro"] = mavlink_msg_raw_imu_get_zgyro(&msg); content["xmag"] = mavlink_msg_raw_imu_get_xmag(&msg); content["ymag"] = mavlink_msg_raw_imu_get_ymag(&msg); content["zmag"] = mavlink_msg_raw_imu_get_zmag(&msg); break; } case 28: { //RAW_PRESSURE content["time_usec"] = mavlink_msg_raw_pressure_get_time_usec(&msg); content["press_abs"] = mavlink_msg_raw_pressure_get_press_abs(&msg); content["press_diff1"] = mavlink_msg_raw_pressure_get_press_diff1(&msg); content["press_diff2"] = mavlink_msg_raw_pressure_get_press_diff2(&msg); content["temperature"] = mavlink_msg_raw_pressure_get_temperature(&msg); break; } case 29: { //SCALED_PRESSURE content["time_boot_ms"] = mavlink_msg_scaled_pressure_get_time_boot_ms(&msg); content["press_abs"] = mavlink_msg_scaled_pressure_get_press_abs(&msg); content["press_diff"] = mavlink_msg_scaled_pressure_get_press_diff(&msg); content["temperature"] = mavlink_msg_scaled_pressure_get_temperature(&msg); break; } case 30: { //ATTITUDE content["time_boot_ms"] = mavlink_msg_attitude_get_time_boot_ms(&msg); content["roll"] = mavlink_msg_attitude_get_roll(&msg); content["pitch"] = mavlink_msg_attitude_get_pitch(&msg); content["yaw"] = mavlink_msg_attitude_get_yaw(&msg); content["rollspeed"] = mavlink_msg_attitude_get_rollspeed(&msg); content["pitchspeed"] = mavlink_msg_attitude_get_pitchspeed(&msg); content["yawspeed"] = mavlink_msg_attitude_get_yawspeed(&msg); break; } case 31: { //ATTITUDE_QUATERNION content["time_boot_ms"] = mavlink_msg_attitude_quaternion_get_time_boot_ms(&msg); content["q1"] = mavlink_msg_attitude_quaternion_get_q1(&msg); content["q2"] = mavlink_msg_attitude_quaternion_get_q2(&msg); content["q3"] = mavlink_msg_attitude_quaternion_get_q3(&msg); content["q4"] = mavlink_msg_attitude_quaternion_get_q4(&msg); content["rollspeed"] = mavlink_msg_attitude_quaternion_get_rollspeed(&msg); content["pitchspeed"] = mavlink_msg_attitude_quaternion_get_pitchspeed(&msg); content["yawspeed"] = mavlink_msg_attitude_quaternion_get_yawspeed(&msg); break; } case 32: { //LOCAL_POSITION_NED content["time_boot_ms"] = mavlink_msg_local_position_ned_get_time_boot_ms(&msg); content["x"] = mavlink_msg_local_position_ned_get_x(&msg); content["y"] = mavlink_msg_local_position_ned_get_y(&msg); content["z"] = mavlink_msg_local_position_ned_get_z(&msg); content["vx"] = mavlink_msg_local_position_ned_get_vx(&msg); content["vy"] = mavlink_msg_local_position_ned_get_vy(&msg); content["vz"] = mavlink_msg_local_position_ned_get_vz(&msg); break; } case 33: { //GLOBAL_POSITION_INT content["time_boot_ms"] = mavlink_msg_global_position_int_get_time_boot_ms(&msg); content["lat"] = mavlink_msg_global_position_int_get_lat(&msg); content["lon"] = mavlink_msg_global_position_int_get_lon(&msg); content["alt"] = mavlink_msg_global_position_int_get_alt(&msg); content["relative_alt"] = mavlink_msg_global_position_int_get_relative_alt(&msg); content["vx"] = mavlink_msg_global_position_int_get_vx(&msg); content["vy"] = mavlink_msg_global_position_int_get_vy(&msg); content["vz"] = mavlink_msg_global_position_int_get_vz(&msg); content["hdg"] = mavlink_msg_global_position_int_get_hdg(&msg); break; } case 34: { //RC_CHANNELS_SCALED content["time_boot_ms"] = mavlink_msg_rc_channels_scaled_get_time_boot_ms(&msg); content["port"] = mavlink_msg_rc_channels_scaled_get_port(&msg); content["chan1_scaled"] = mavlink_msg_rc_channels_scaled_get_chan1_scaled(&msg); content["chan2_scaled"] = mavlink_msg_rc_channels_scaled_get_chan2_scaled(&msg); content["chan3_scaled"] = mavlink_msg_rc_channels_scaled_get_chan3_scaled(&msg); content["chan4_scaled"] = mavlink_msg_rc_channels_scaled_get_chan4_scaled(&msg); content["chan5_scaled"] = mavlink_msg_rc_channels_scaled_get_chan5_scaled(&msg); content["chan6_scaled"] = mavlink_msg_rc_channels_scaled_get_chan6_scaled(&msg); content["chan7_scaled"] = mavlink_msg_rc_channels_scaled_get_chan7_scaled(&msg); content["chan8_scaled"] = mavlink_msg_rc_channels_scaled_get_chan8_scaled(&msg); content["rssi"] = mavlink_msg_rc_channels_scaled_get_rssi(&msg); break; } case 35: { //RC_CHANNELS_RAW content["time_boot_ms"] = mavlink_msg_rc_channels_raw_get_time_boot_ms(&msg); content["port"] = mavlink_msg_rc_channels_raw_get_port(&msg); content["chan1_raw"] = mavlink_msg_rc_channels_raw_get_chan1_raw(&msg); content["chan2_raw"] = mavlink_msg_rc_channels_raw_get_chan2_raw(&msg); content["chan3_raw"] = mavlink_msg_rc_channels_raw_get_chan3_raw(&msg); content["chan4_raw"] = mavlink_msg_rc_channels_raw_get_chan4_raw(&msg); content["chan5_raw"] = mavlink_msg_rc_channels_raw_get_chan5_raw(&msg); content["chan6_raw"] = mavlink_msg_rc_channels_raw_get_chan6_raw(&msg); content["chan7_raw"] = mavlink_msg_rc_channels_raw_get_chan7_raw(&msg); content["chan8_raw"] = mavlink_msg_rc_channels_raw_get_chan8_raw(&msg); content["rssi"] = mavlink_msg_rc_channels_raw_get_rssi(&msg); break; } case 36: { //SERVO_OUTPUT_RAW content["time_usec"] = mavlink_msg_servo_output_raw_get_time_usec(&msg); content["port"] = mavlink_msg_servo_output_raw_get_port(&msg); content["servo1_raw"] = mavlink_msg_servo_output_raw_get_servo1_raw(&msg); content["servo2_raw"] = mavlink_msg_servo_output_raw_get_servo2_raw(&msg); content["servo3_raw"] = mavlink_msg_servo_output_raw_get_servo3_raw(&msg); content["servo4_raw"] = mavlink_msg_servo_output_raw_get_servo4_raw(&msg); content["servo5_raw"] = mavlink_msg_servo_output_raw_get_servo5_raw(&msg); content["servo6_raw"] = mavlink_msg_servo_output_raw_get_servo6_raw(&msg); content["servo7_raw"] = mavlink_msg_servo_output_raw_get_servo7_raw(&msg); content["servo8_raw"] = mavlink_msg_servo_output_raw_get_servo8_raw(&msg); break; } case 37: { //MISSION_REQUEST_PARTIAL_LIST content["target_system"] = mavlink_msg_mission_request_partial_list_get_target_system(&msg); content["target_component"] = mavlink_msg_mission_request_partial_list_get_target_component(&msg); content["start_index"] = mavlink_msg_mission_request_partial_list_get_start_index(&msg); content["end_index"] = mavlink_msg_mission_request_partial_list_get_end_index(&msg); break; } case 38: { //MISSION_WRITE_PARTIAL_LIST content["target_system"] = mavlink_msg_mission_write_partial_list_get_target_system(&msg); content["target_component"] = mavlink_msg_mission_write_partial_list_get_target_component(&msg); content["start_index"] = mavlink_msg_mission_write_partial_list_get_start_index(&msg); content["end_index"] = mavlink_msg_mission_write_partial_list_get_end_index(&msg); break; } case 39: { //MISSION_ITEM content["target_system"] = mavlink_msg_mission_item_get_target_system(&msg); content["target_component"] = mavlink_msg_mission_item_get_target_component(&msg); content["seq"] = mavlink_msg_mission_item_get_seq(&msg); content["frame"] = mavlink_msg_mission_item_get_frame(&msg); content["command"] = mavlink_msg_mission_item_get_command(&msg); content["current"] = mavlink_msg_mission_item_get_current(&msg); content["autocontinue"] = mavlink_msg_mission_item_get_autocontinue(&msg); content["param1"] = mavlink_msg_mission_item_get_param1(&msg); content["param2"] = mavlink_msg_mission_item_get_param2(&msg); content["param3"] = mavlink_msg_mission_item_get_param3(&msg); content["param4"] = mavlink_msg_mission_item_get_param4(&msg); content["x"] = mavlink_msg_mission_item_get_x(&msg); content["y"] = mavlink_msg_mission_item_get_y(&msg); content["z"] = mavlink_msg_mission_item_get_z(&msg); break; } case 40: { //MISSION_REQUEST content["target_system"] = mavlink_msg_mission_request_get_target_system(&msg); content["target_component"] = mavlink_msg_mission_request_get_target_component(&msg); content["seq"] = mavlink_msg_mission_request_get_seq(&msg); break; } case 41: { //MISSION_SET_CURRENT content["target_system"] = mavlink_msg_mission_set_current_get_target_system(&msg); content["target_component"] = mavlink_msg_mission_set_current_get_target_component(&msg); content["seq"] = mavlink_msg_mission_set_current_get_seq(&msg); break; } case 42: { //MISSION_CURRENT content["seq"] = mavlink_msg_mission_current_get_seq(&msg); break; } case 43: { //MISSION_REQUEST_LIST content["target_system"] = mavlink_msg_mission_request_list_get_target_system(&msg); content["target_component"] = mavlink_msg_mission_request_list_get_target_component(&msg); break; } case 44: { //MISSION_COUNT content["target_system"] = mavlink_msg_mission_count_get_target_system(&msg); content["target_component"] = mavlink_msg_mission_count_get_target_component(&msg); content["count"] = mavlink_msg_mission_count_get_count(&msg); break; } case 45: { //MISSION_CLEAR_ALL content["target_system"] = mavlink_msg_mission_clear_all_get_target_system(&msg); content["target_component"] = mavlink_msg_mission_clear_all_get_target_component(&msg); break; } case 46: { //MISSION_ITEM_REACHED content["seq"] = mavlink_msg_mission_item_reached_get_seq(&msg); break; } case 47: { //MISSION_ACK content["target_system"] = mavlink_msg_mission_ack_get_target_system(&msg); content["target_component"] = mavlink_msg_mission_ack_get_target_component(&msg); content["type"] = mavlink_msg_mission_ack_get_type(&msg); break; } case 48: { //SET_GPS_GLOBAL_ORIGIN content["target_system"] = mavlink_msg_set_gps_global_origin_get_target_system(&msg); content["latitude"] = mavlink_msg_set_gps_global_origin_get_latitude(&msg); content["longitude"] = mavlink_msg_set_gps_global_origin_get_longitude(&msg); content["altitude"] = mavlink_msg_set_gps_global_origin_get_altitude(&msg); break; } case 49: { //GPS_GLOBAL_ORIGIN content["latitude"] = mavlink_msg_gps_global_origin_get_latitude(&msg); content["longitude"] = mavlink_msg_gps_global_origin_get_longitude(&msg); content["altitude"] = mavlink_msg_gps_global_origin_get_altitude(&msg); break; } case 50: { //SET_LOCAL_POSITION_SETPOINT content["target_system"] = mavlink_msg_set_local_position_setpoint_get_target_system(&msg); content["target_component"] = mavlink_msg_set_local_position_setpoint_get_target_component(&msg); content["coordinate_frame"] = mavlink_msg_set_local_position_setpoint_get_coordinate_frame(&msg); content["x"] = mavlink_msg_set_local_position_setpoint_get_x(&msg); content["y"] = mavlink_msg_set_local_position_setpoint_get_y(&msg); content["z"] = mavlink_msg_set_local_position_setpoint_get_z(&msg); content["yaw"] = mavlink_msg_set_local_position_setpoint_get_yaw(&msg); break; } case 51: { //LOCAL_POSITION_SETPOINT content["coordinate_frame"] = mavlink_msg_local_position_setpoint_get_coordinate_frame(&msg); content["x"] = mavlink_msg_local_position_setpoint_get_x(&msg); content["y"] = mavlink_msg_local_position_setpoint_get_y(&msg); content["z"] = mavlink_msg_local_position_setpoint_get_z(&msg); content["yaw"] = mavlink_msg_local_position_setpoint_get_yaw(&msg); break; } case 52: { //GLOBAL_POSITION_SETPOINT_INT content["coordinate_frame"] = mavlink_msg_global_position_setpoint_int_get_coordinate_frame(&msg); content["latitude"] = mavlink_msg_global_position_setpoint_int_get_latitude(&msg); content["longitude"] = mavlink_msg_global_position_setpoint_int_get_longitude(&msg); content["altitude"] = mavlink_msg_global_position_setpoint_int_get_altitude(&msg); content["yaw"] = mavlink_msg_global_position_setpoint_int_get_yaw(&msg); break; } case 53: { //SET_GLOBAL_POSITION_SETPOINT_INT content["coordinate_frame"] = mavlink_msg_set_global_position_setpoint_int_get_coordinate_frame(&msg); content["latitude"] = mavlink_msg_set_global_position_setpoint_int_get_latitude(&msg); content["longitude"] = mavlink_msg_set_global_position_setpoint_int_get_longitude(&msg); content["altitude"] = mavlink_msg_set_global_position_setpoint_int_get_altitude(&msg); content["yaw"] = mavlink_msg_set_global_position_setpoint_int_get_yaw(&msg); break; } case 54: { //SAFETY_SET_ALLOWED_AREA content["target_system"] = mavlink_msg_safety_set_allowed_area_get_target_system(&msg); content["target_component"] = mavlink_msg_safety_set_allowed_area_get_target_component(&msg); content["frame"] = mavlink_msg_safety_set_allowed_area_get_frame(&msg); content["p1x"] = mavlink_msg_safety_set_allowed_area_get_p1x(&msg); content["p1y"] = mavlink_msg_safety_set_allowed_area_get_p1y(&msg); content["p1z"] = mavlink_msg_safety_set_allowed_area_get_p1z(&msg); content["p2x"] = mavlink_msg_safety_set_allowed_area_get_p2x(&msg); content["p2y"] = mavlink_msg_safety_set_allowed_area_get_p2y(&msg); content["p2z"] = mavlink_msg_safety_set_allowed_area_get_p2z(&msg); break; } case 55: { //SAFETY_ALLOWED_AREA content["frame"] = mavlink_msg_safety_allowed_area_get_frame(&msg); content["p1x"] = mavlink_msg_safety_allowed_area_get_p1x(&msg); content["p1y"] = mavlink_msg_safety_allowed_area_get_p1y(&msg); content["p1z"] = mavlink_msg_safety_allowed_area_get_p1z(&msg); content["p2x"] = mavlink_msg_safety_allowed_area_get_p2x(&msg); content["p2y"] = mavlink_msg_safety_allowed_area_get_p2y(&msg); content["p2z"] = mavlink_msg_safety_allowed_area_get_p2z(&msg); break; } case 56: { //SET_ROLL_PITCH_YAW_THRUST content["target_system"] = mavlink_msg_set_roll_pitch_yaw_thrust_get_target_system(&msg); content["target_component"] = mavlink_msg_set_roll_pitch_yaw_thrust_get_target_component(&msg); content["roll"] = mavlink_msg_set_roll_pitch_yaw_thrust_get_roll(&msg); content["pitch"] = mavlink_msg_set_roll_pitch_yaw_thrust_get_pitch(&msg); content["yaw"] = mavlink_msg_set_roll_pitch_yaw_thrust_get_yaw(&msg); content["thrust"] = mavlink_msg_set_roll_pitch_yaw_thrust_get_thrust(&msg); break; } case 57: { //SET_ROLL_PITCH_YAW_SPEED_THRUST content["target_system"] = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_system(&msg); content["target_component"] = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_component(&msg); content["roll_speed"] = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_roll_speed(&msg); content["pitch_speed"] = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_pitch_speed(&msg); content["yaw_speed"] = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_yaw_speed(&msg); content["thrust"] = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_thrust(&msg); break; } case 58: { //ROLL_PITCH_YAW_THRUST_SETPOINT content["time_boot_ms"] = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_time_boot_ms(&msg); content["roll"] = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_roll(&msg); content["pitch"] = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_pitch(&msg); content["yaw"] = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_yaw(&msg); content["thrust"] = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_thrust(&msg); break; } case 59: { //ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT content["time_boot_ms"] = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_time_boot_ms(&msg); content["roll_speed"] = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_roll_speed(&msg); content["pitch_speed"] = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_pitch_speed(&msg); content["yaw_speed"] = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_yaw_speed(&msg); content["thrust"] = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_thrust(&msg); break; } case 60: { //SET_QUAD_MOTORS_SETPOINT content["target_system"] = mavlink_msg_set_quad_motors_setpoint_get_target_system(&msg); content["motor_front_nw"] = mavlink_msg_set_quad_motors_setpoint_get_motor_front_nw(&msg); content["motor_right_ne"] = mavlink_msg_set_quad_motors_setpoint_get_motor_right_ne(&msg); content["motor_back_se"] = mavlink_msg_set_quad_motors_setpoint_get_motor_back_se(&msg); content["motor_left_sw"] = mavlink_msg_set_quad_motors_setpoint_get_motor_left_sw(&msg); break; } case 61: { //SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST uint8_t arr_target_systems[6]; mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_target_systems(&msg, arr_target_systems); content["target_systems"] = Json::Value(Json::arrayValue); for(uint8_t i : arr_target_systems) { content["target_systems"].append(i); } int16_t arr_roll[6]; mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_roll(&msg, arr_roll); content["roll"] = Json::Value(Json::arrayValue); for(int16_t i : arr_roll) { content["roll"].append(i); } int16_t arr_pitch[6]; mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_pitch(&msg, arr_pitch); content["pitch"] = Json::Value(Json::arrayValue); for(int16_t i : arr_pitch) { content["pitch"].append(i); } int16_t arr_yaw[6]; mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_yaw(&msg, arr_yaw); content["yaw"] = Json::Value(Json::arrayValue); for(int16_t i : arr_yaw) { content["yaw"].append(i); } int16_t arr_thrust[6]; mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_thrust(&msg, arr_thrust); content["thrust"] = Json::Value(Json::arrayValue); for(int16_t i : arr_thrust) { content["thrust"].append(i); } break; } case 62: { //NAV_CONTROLLER_OUTPUT content["nav_roll"] = mavlink_msg_nav_controller_output_get_nav_roll(&msg); content["nav_pitch"] = mavlink_msg_nav_controller_output_get_nav_pitch(&msg); content["nav_bearing"] = mavlink_msg_nav_controller_output_get_nav_bearing(&msg); content["target_bearing"] = mavlink_msg_nav_controller_output_get_target_bearing(&msg); content["wp_dist"] = mavlink_msg_nav_controller_output_get_wp_dist(&msg); content["alt_error"] = mavlink_msg_nav_controller_output_get_alt_error(&msg); content["aspd_error"] = mavlink_msg_nav_controller_output_get_aspd_error(&msg); content["xtrack_error"] = mavlink_msg_nav_controller_output_get_xtrack_error(&msg); break; } case 64: { //STATE_CORRECTION content["xErr"] = mavlink_msg_state_correction_get_xErr(&msg); content["yErr"] = mavlink_msg_state_correction_get_yErr(&msg); content["zErr"] = mavlink_msg_state_correction_get_zErr(&msg); content["rollErr"] = mavlink_msg_state_correction_get_rollErr(&msg); content["pitchErr"] = mavlink_msg_state_correction_get_pitchErr(&msg); content["yawErr"] = mavlink_msg_state_correction_get_yawErr(&msg); content["vxErr"] = mavlink_msg_state_correction_get_vxErr(&msg); content["vyErr"] = mavlink_msg_state_correction_get_vyErr(&msg); content["vzErr"] = mavlink_msg_state_correction_get_vzErr(&msg); break; } case 66: { //REQUEST_DATA_STREAM content["target_system"] = mavlink_msg_request_data_stream_get_target_system(&msg); content["target_component"] = mavlink_msg_request_data_stream_get_target_component(&msg); content["req_stream_id"] = mavlink_msg_request_data_stream_get_req_stream_id(&msg); content["req_message_rate"] = mavlink_msg_request_data_stream_get_req_message_rate(&msg); content["start_stop"] = mavlink_msg_request_data_stream_get_start_stop(&msg); break; } case 67: { //DATA_STREAM content["stream_id"] = mavlink_msg_data_stream_get_stream_id(&msg); content["message_rate"] = mavlink_msg_data_stream_get_message_rate(&msg); content["on_off"] = mavlink_msg_data_stream_get_on_off(&msg); break; } case 69: { //MANUAL_CONTROL content["target"] = mavlink_msg_manual_control_get_target(&msg); content["roll"] = mavlink_msg_manual_control_get_roll(&msg); content["pitch"] = mavlink_msg_manual_control_get_pitch(&msg); content["yaw"] = mavlink_msg_manual_control_get_yaw(&msg); content["thrust"] = mavlink_msg_manual_control_get_thrust(&msg); content["roll_manual"] = mavlink_msg_manual_control_get_roll_manual(&msg); content["pitch_manual"] = mavlink_msg_manual_control_get_pitch_manual(&msg); content["yaw_manual"] = mavlink_msg_manual_control_get_yaw_manual(&msg); content["thrust_manual"] = mavlink_msg_manual_control_get_thrust_manual(&msg); break; } case 70: { //RC_CHANNELS_OVERRIDE content["target_system"] = mavlink_msg_rc_channels_override_get_target_system(&msg); content["target_component"] = mavlink_msg_rc_channels_override_get_target_component(&msg); content["chan1_raw"] = mavlink_msg_rc_channels_override_get_chan1_raw(&msg); content["chan2_raw"] = mavlink_msg_rc_channels_override_get_chan2_raw(&msg); content["chan3_raw"] = mavlink_msg_rc_channels_override_get_chan3_raw(&msg); content["chan4_raw"] = mavlink_msg_rc_channels_override_get_chan4_raw(&msg); content["chan5_raw"] = mavlink_msg_rc_channels_override_get_chan5_raw(&msg); content["chan6_raw"] = mavlink_msg_rc_channels_override_get_chan6_raw(&msg); content["chan7_raw"] = mavlink_msg_rc_channels_override_get_chan7_raw(&msg); content["chan8_raw"] = mavlink_msg_rc_channels_override_get_chan8_raw(&msg); break; } case 74: { //VFR_HUD content["airspeed"] = mavlink_msg_vfr_hud_get_airspeed(&msg); content["groundspeed"] = mavlink_msg_vfr_hud_get_groundspeed(&msg); content["heading"] = mavlink_msg_vfr_hud_get_heading(&msg); content["throttle"] = mavlink_msg_vfr_hud_get_throttle(&msg); content["alt"] = mavlink_msg_vfr_hud_get_alt(&msg); content["climb"] = mavlink_msg_vfr_hud_get_climb(&msg); break; } case 76: { //COMMAND_LONG content["target_system"] = mavlink_msg_command_long_get_target_system(&msg); content["target_component"] = mavlink_msg_command_long_get_target_component(&msg); content["command"] = mavlink_msg_command_long_get_command(&msg); content["confirmation"] = mavlink_msg_command_long_get_confirmation(&msg); content["param1"] = mavlink_msg_command_long_get_param1(&msg); content["param2"] = mavlink_msg_command_long_get_param2(&msg); content["param3"] = mavlink_msg_command_long_get_param3(&msg); content["param4"] = mavlink_msg_command_long_get_param4(&msg); content["param5"] = mavlink_msg_command_long_get_param5(&msg); content["param6"] = mavlink_msg_command_long_get_param6(&msg); content["param7"] = mavlink_msg_command_long_get_param7(&msg); break; } case 77: { //COMMAND_ACK content["command"] = mavlink_msg_command_ack_get_command(&msg); content["result"] = mavlink_msg_command_ack_get_result(&msg); break; } case 89: { //LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET content["time_boot_ms"] = mavlink_msg_local_position_ned_system_global_offset_get_time_boot_ms(&msg); content["x"] = mavlink_msg_local_position_ned_system_global_offset_get_x(&msg); content["y"] = mavlink_msg_local_position_ned_system_global_offset_get_y(&msg); content["z"] = mavlink_msg_local_position_ned_system_global_offset_get_z(&msg); content["roll"] = mavlink_msg_local_position_ned_system_global_offset_get_roll(&msg); content["pitch"] = mavlink_msg_local_position_ned_system_global_offset_get_pitch(&msg); content["yaw"] = mavlink_msg_local_position_ned_system_global_offset_get_yaw(&msg); break; } case 90: { //HIL_STATE content["time_usec"] = mavlink_msg_hil_state_get_time_usec(&msg); content["roll"] = mavlink_msg_hil_state_get_roll(&msg); content["pitch"] = mavlink_msg_hil_state_get_pitch(&msg); content["yaw"] = mavlink_msg_hil_state_get_yaw(&msg); content["rollspeed"] = mavlink_msg_hil_state_get_rollspeed(&msg); content["pitchspeed"] = mavlink_msg_hil_state_get_pitchspeed(&msg); content["yawspeed"] = mavlink_msg_hil_state_get_yawspeed(&msg); content["lat"] = mavlink_msg_hil_state_get_lat(&msg); content["lon"] = mavlink_msg_hil_state_get_lon(&msg); content["alt"] = mavlink_msg_hil_state_get_alt(&msg); content["vx"] = mavlink_msg_hil_state_get_vx(&msg); content["vy"] = mavlink_msg_hil_state_get_vy(&msg); content["vz"] = mavlink_msg_hil_state_get_vz(&msg); content["xacc"] = mavlink_msg_hil_state_get_xacc(&msg); content["yacc"] = mavlink_msg_hil_state_get_yacc(&msg); content["zacc"] = mavlink_msg_hil_state_get_zacc(&msg); break; } case 91: { //HIL_CONTROLS content["time_usec"] = mavlink_msg_hil_controls_get_time_usec(&msg); content["roll_ailerons"] = mavlink_msg_hil_controls_get_roll_ailerons(&msg); content["pitch_elevator"] = mavlink_msg_hil_controls_get_pitch_elevator(&msg); content["yaw_rudder"] = mavlink_msg_hil_controls_get_yaw_rudder(&msg); content["throttle"] = mavlink_msg_hil_controls_get_throttle(&msg); content["aux1"] = mavlink_msg_hil_controls_get_aux1(&msg); content["aux2"] = mavlink_msg_hil_controls_get_aux2(&msg); content["aux3"] = mavlink_msg_hil_controls_get_aux3(&msg); content["aux4"] = mavlink_msg_hil_controls_get_aux4(&msg); content["mode"] = mavlink_msg_hil_controls_get_mode(&msg); content["nav_mode"] = mavlink_msg_hil_controls_get_nav_mode(&msg); break; } case 92: { //HIL_RC_INPUTS_RAW content["time_usec"] = mavlink_msg_hil_rc_inputs_raw_get_time_usec(&msg); content["chan1_raw"] = mavlink_msg_hil_rc_inputs_raw_get_chan1_raw(&msg); content["chan2_raw"] = mavlink_msg_hil_rc_inputs_raw_get_chan2_raw(&msg); content["chan3_raw"] = mavlink_msg_hil_rc_inputs_raw_get_chan3_raw(&msg); content["chan4_raw"] = mavlink_msg_hil_rc_inputs_raw_get_chan4_raw(&msg); content["chan5_raw"] = mavlink_msg_hil_rc_inputs_raw_get_chan5_raw(&msg); content["chan6_raw"] = mavlink_msg_hil_rc_inputs_raw_get_chan6_raw(&msg); content["chan7_raw"] = mavlink_msg_hil_rc_inputs_raw_get_chan7_raw(&msg); content["chan8_raw"] = mavlink_msg_hil_rc_inputs_raw_get_chan8_raw(&msg); content["chan9_raw"] = mavlink_msg_hil_rc_inputs_raw_get_chan9_raw(&msg); content["chan10_raw"] = mavlink_msg_hil_rc_inputs_raw_get_chan10_raw(&msg); content["chan11_raw"] = mavlink_msg_hil_rc_inputs_raw_get_chan11_raw(&msg); content["chan12_raw"] = mavlink_msg_hil_rc_inputs_raw_get_chan12_raw(&msg); content["rssi"] = mavlink_msg_hil_rc_inputs_raw_get_rssi(&msg); break; } case 100: { //OPTICAL_FLOW content["time_usec"] = mavlink_msg_optical_flow_get_time_usec(&msg); content["sensor_id"] = mavlink_msg_optical_flow_get_sensor_id(&msg); content["flow_x"] = mavlink_msg_optical_flow_get_flow_x(&msg); content["flow_y"] = mavlink_msg_optical_flow_get_flow_y(&msg); content["flow_comp_m_x"] = mavlink_msg_optical_flow_get_flow_comp_m_x(&msg); content["flow_comp_m_y"] = mavlink_msg_optical_flow_get_flow_comp_m_y(&msg); content["quality"] = mavlink_msg_optical_flow_get_quality(&msg); content["ground_distance"] = mavlink_msg_optical_flow_get_ground_distance(&msg); break; } case 101: { //GLOBAL_VISION_POSITION_ESTIMATE content["usec"] = mavlink_msg_global_vision_position_estimate_get_usec(&msg); content["x"] = mavlink_msg_global_vision_position_estimate_get_x(&msg); content["y"] = mavlink_msg_global_vision_position_estimate_get_y(&msg); content["z"] = mavlink_msg_global_vision_position_estimate_get_z(&msg); content["roll"] = mavlink_msg_global_vision_position_estimate_get_roll(&msg); content["pitch"] = mavlink_msg_global_vision_position_estimate_get_pitch(&msg); content["yaw"] = mavlink_msg_global_vision_position_estimate_get_yaw(&msg); break; } case 102: { //VISION_POSITION_ESTIMATE content["usec"] = mavlink_msg_vision_position_estimate_get_usec(&msg); content["x"] = mavlink_msg_vision_position_estimate_get_x(&msg); content["y"] = mavlink_msg_vision_position_estimate_get_y(&msg); content["z"] = mavlink_msg_vision_position_estimate_get_z(&msg); content["roll"] = mavlink_msg_vision_position_estimate_get_roll(&msg); content["pitch"] = mavlink_msg_vision_position_estimate_get_pitch(&msg); content["yaw"] = mavlink_msg_vision_position_estimate_get_yaw(&msg); break; } case 103: { //VISION_SPEED_ESTIMATE content["usec"] = mavlink_msg_vision_speed_estimate_get_usec(&msg); content["x"] = mavlink_msg_vision_speed_estimate_get_x(&msg); content["y"] = mavlink_msg_vision_speed_estimate_get_y(&msg); content["z"] = mavlink_msg_vision_speed_estimate_get_z(&msg); break; } case 104: { //VICON_POSITION_ESTIMATE content["usec"] = mavlink_msg_vicon_position_estimate_get_usec(&msg); content["x"] = mavlink_msg_vicon_position_estimate_get_x(&msg); content["y"] = mavlink_msg_vicon_position_estimate_get_y(&msg); content["z"] = mavlink_msg_vicon_position_estimate_get_z(&msg); content["roll"] = mavlink_msg_vicon_position_estimate_get_roll(&msg); content["pitch"] = mavlink_msg_vicon_position_estimate_get_pitch(&msg); content["yaw"] = mavlink_msg_vicon_position_estimate_get_yaw(&msg); break; } case 249: { //MEMORY_VECT content["address"] = mavlink_msg_memory_vect_get_address(&msg); content["ver"] = mavlink_msg_memory_vect_get_ver(&msg); content["type"] = mavlink_msg_memory_vect_get_type(&msg); content["value"] = mavlink_msg_memory_vect_get_value(&msg); break; } case 250: { //DEBUG_VECT char arr_name[10]; mavlink_msg_debug_vect_get_name(&msg, arr_name); content["name"] = std::string(arr_name, 10); content["time_usec"] = mavlink_msg_debug_vect_get_time_usec(&msg); content["x"] = mavlink_msg_debug_vect_get_x(&msg); content["y"] = mavlink_msg_debug_vect_get_y(&msg); content["z"] = mavlink_msg_debug_vect_get_z(&msg); break; } case 251: { //NAMED_VALUE_FLOAT content["time_boot_ms"] = mavlink_msg_named_value_float_get_time_boot_ms(&msg); char arr_name[10]; mavlink_msg_named_value_float_get_name(&msg, arr_name); content["name"] = std::string(arr_name, 10); content["value"] = mavlink_msg_named_value_float_get_value(&msg); break; } case 252: { //NAMED_VALUE_INT content["time_boot_ms"] = mavlink_msg_named_value_int_get_time_boot_ms(&msg); char arr_name[10]; mavlink_msg_named_value_int_get_name(&msg, arr_name); content["name"] = std::string(arr_name, 10); content["value"] = mavlink_msg_named_value_int_get_value(&msg); break; } case 253: { //STATUSTEXT content["severity"] = mavlink_msg_statustext_get_severity(&msg); char arr_text[50]; mavlink_msg_statustext_get_text(&msg, arr_text); content["text"] = std::string(arr_text, 50); break; } case 254: { //DEBUG content["time_boot_ms"] = mavlink_msg_debug_get_time_boot_ms(&msg); content["ind"] = mavlink_msg_debug_get_ind(&msg); content["value"] = mavlink_msg_debug_get_value(&msg); break; } } return content; }
int main (void) { char textBuffer[50]; uint8_t batReqs=0; uint8_t heartbeats=0; uint8_t statusFlag=0; uint8_t gpsFlag=0; uint8_t batFlag=0; setCurBuf(0); setPwrBuf(0,0); setAltBuf(0); setAirBuf(0); setGPSBuf(0,0,0,0,0,0); TWISlaveInit(); #ifdef TIMEIT TimerInit(); #endif mavlink_system.sysid = 1; mavlink_system.compid = 50; USART_Init(); mavlink_message_t msg; mavlink_status_t status; while(1) { uint8_t byte=usartGetChar(); if(mavlink_parse_char(MAVLINK_COMM_0, byte,&msg,&status)) { switch(msg.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: heartbeats++; for (int n=1;n<10;n++) PORTB ^= (1<<PB5); // After a couple of heartbeats, try up to 3 times to get the battery capacity if((heartbeats>2)&&(!batFlag)&&(batReqs<3)) { mavlink_msg_param_request_read_send(MAVLINK_COMM_0,0x01,0x00,capName,-1); batReqs++; } // if you go 2 heartbeats without getting the status or GPS message, re-request the telemetry stream if(((!statusFlag)||(!gpsFlag))&&(heartbeats>2)) { mavlink_msg_request_data_stream_send(MAVLINK_COMM_0, 0x01, 0x00, 0x01, 0x200, 0x01); heartbeats=0; } else { statusFlag=0; gpsFlag=0; } break; case MAVLINK_MSG_ID_PARAM_VALUE: mavlink_msg_param_value_get_param_id(&msg,textBuffer); float value=mavlink_msg_param_value_get_param_value(&msg); if(strncmp(textBuffer,capName,16)==0) { batteryCapacity=(uint16_t)value; batFlag=1; } break; case MAVLINK_MSG_ID_SYS_STATUS: setPwrBuf(mavlink_msg_sys_status_get_voltage_battery(&msg),mavlink_msg_sys_status_get_battery_remaining(&msg)); setCurBuf(mavlink_msg_sys_status_get_current_battery(&msg)); statusFlag=1; break; case MAVLINK_MSG_ID_GPS_RAW_INT: setAltBuf(mavlink_msg_gps_raw_int_get_alt(&msg)); #ifdef TIMEIT TCNT1=0; #endif setGPSBuf(mavlink_msg_gps_raw_int_get_lat(&msg),mavlink_msg_gps_raw_int_get_lon(&msg),mavlink_msg_gps_raw_int_get_alt(&msg), mavlink_msg_gps_raw_int_get_vel(&msg),mavlink_msg_gps_raw_int_get_cog(&msg), mavlink_msg_gps_raw_int_get_satellites_visible(&msg)); #ifdef TIMEIT timeVal=TCNT1; #endif setAirBuf(mavlink_msg_gps_raw_int_get_vel(&msg)); gpsFlag=1; break; case MAVLINK_MSG_ID_STATUSTEXT: mavlink_msg_statustext_get_text(&msg,textBuffer); // Set status for Powerbox when the low battery message is received if(strncmp(textBuffer,lowBattery,strlen(lowBattery))==0) alarm |= (1<<LOW_BATTERY); break; default: break; } } } }
void MavlinkProcessor::receiveTelemetry() { tryToConnectToAPM(); while(MavLinkSerial.available()) { uint8_t c = MavLinkSerial.read(); if(mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) { switch(msg.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: // 0 if (msg.sysid == 1) { gathered_telemetry.base_mode = mavlink_msg_heartbeat_get_base_mode(&msg); gathered_telemetry.custom_mode = mavlink_msg_heartbeat_get_custom_mode(&msg); connection_timer = millis(); if(!is_connected); { heartbeat_count++; if((heartbeat_count) > 10) { // If received > 10 heartbeats from MavLink then we are connected is_connected = true; heartbeat_count = 0; digitalWrite(alive_led_pin, HIGH); // LED will be ON when connected to MavLink, else it will slowly blink } } } break; case MAVLINK_MSG_ID_SYS_STATUS : // 1 gathered_telemetry.battery_voltage = mavlink_msg_sys_status_get_voltage_battery(&msg) / 10; // gathered_telemetry.battery_current = mavlink_msg_sys_status_get_current_battery(&msg) / 10; // gathered_telemetry.battery_remaining = mavlink_msg_sys_status_get_battery_remaining(&msg); break; case MAVLINK_MSG_ID_GPS_RAW_INT: // 24 gathered_telemetry.gps_fixtype = mavlink_msg_gps_raw_int_get_fix_type(&msg);// 0 = No GPS, 1 =No Fix, 2 = 2D Fix, 3 = 3D Fix gathered_telemetry.gps_satellites_visible = mavlink_msg_gps_raw_int_get_satellites_visible(&msg); // numbers of visible satelites if(gathered_telemetry.gps_fixtype == 3) { gathered_telemetry.gps_hdop = mavlink_msg_gps_raw_int_get_eph(&msg); // hdop * 100 gathered_telemetry.gps_latitude = mavlink_msg_gps_raw_int_get_lat(&msg); gathered_telemetry.gps_longitude = mavlink_msg_gps_raw_int_get_lon(&msg); gathered_telemetry.gps_altitude = mavlink_msg_gps_raw_int_get_alt(&msg); // 1m =1000 gathered_telemetry.gps_speed = mavlink_msg_gps_raw_int_get_vel(&msg); } else { gathered_telemetry.gps_hdop = 9999; gathered_telemetry.gps_latitude = 0L; gathered_telemetry.gps_longitude = 0l; gathered_telemetry.gps_altitude = 0L; gathered_telemetry.gps_speed = 0L; } break; case MAVLINK_MSG_ID_VFR_HUD: // 74 gathered_telemetry.groundspeed = mavlink_msg_vfr_hud_get_groundspeed(&msg); // 100 = 1m/s gathered_telemetry.heading = mavlink_msg_vfr_hud_get_heading(&msg) ; // 100 = 100 deg gathered_telemetry.throttle = mavlink_msg_vfr_hud_get_throttle(&msg); // 100 = 100% gathered_telemetry.bar_altitude = mavlink_msg_vfr_hud_get_alt(&msg); // m gathered_telemetry.climb_rate = mavlink_msg_vfr_hud_get_climb(&msg) * 100; // m/s break; case MAVLINK_MSG_ID_STATUSTEXT: mavlink_msg_statustext_decode(&msg, &gathered_telemetry.status_text); frsky_send_text_message(gathered_telemetry.status_text.text); break; default: break; } } } }
void MavlinkProcessor::receiveTelemetry(Telemetry& gathered_telemetry) { uint32_t next_timeout = millis() + 6; tryToConnectToAPM(); while(MavLinkSerial.available() && millis() < next_timeout){ if (mavlink_parse_char(MAVLINK_COMM_0, MavLinkSerial.read(), &msg, &status)) { switch(msg.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: // 0 if (msg.sysid == 1) { CHECK_CHANGED(gathered_telemetry.base_mode, mavlink_msg_heartbeat_get_base_mode(&msg), BASE_MODE); CHECK_CHANGED(gathered_telemetry.custom_mode, mavlink_msg_heartbeat_get_custom_mode(&msg), CUSTOM_MODE); CHECK_CHANGED(gathered_telemetry.system_status, mavlink_msg_heartbeat_get_system_status(&msg), SYSTEM_STATUS); CHECK_CHANGED(gathered_telemetry.mav_type, mavlink_msg_heartbeat_get_type(&msg), SlowParameters::MAV_TYPE); connection_timer = millis(); if (!is_connected); { heartbeat_count++; if ((heartbeat_count) > 10) { // If received > 10 heartbeats from MavLink then we are connected is_connected = true; heartbeat_count = 0; digitalWrite(alive_led_pin, HIGH); // LED will be ON when connected to MavLink, else it will slowly blink } } } break; case MAVLINK_MSG_ID_SYS_STATUS: // 1 gathered_telemetry.battery_voltage = mavlink_msg_sys_status_get_voltage_battery(&msg) / 10; // gathered_telemetry.battery_current = mavlink_msg_sys_status_get_current_battery(&msg) / 10; // CHECK_CHANGED(gathered_telemetry.battery_remaining, mavlink_msg_sys_status_get_battery_remaining(&msg), BATTERY_REMAINING); break; case MAVLINK_MSG_ID_GPS_RAW_INT: // 24 CHECK_CHANGED(gathered_telemetry.gps_fixtype, mavlink_msg_gps_raw_int_get_fix_type(&msg), GPS_FIX_TYPE_AND_SAT_VISIBLE); // 0 = No GPS, 1 =No Fix, 2 = 2D Fix, 3 = 3D Fix CHECK_CHANGED(gathered_telemetry.gps_satellites_visible, mavlink_msg_gps_raw_int_get_satellites_visible(&msg), GPS_FIX_TYPE_AND_SAT_VISIBLE); // numbers of visible satelites if (gathered_telemetry.gps_fixtype >= 3) { CHECK_CHANGED(gathered_telemetry.gps_hdop, mavlink_msg_gps_raw_int_get_eph(&msg), HDOP_EX); // hdop * 100 gathered_telemetry.gps_latitude = mavlink_msg_gps_raw_int_get_lat(&msg); gathered_telemetry.gps_longitude = mavlink_msg_gps_raw_int_get_lon(&msg); gathered_telemetry.gps_altitude = mavlink_msg_gps_raw_int_get_alt(&msg); // 1m =1000 gathered_telemetry.gps_speed = mavlink_msg_gps_raw_int_get_vel(&msg); } else { CHECK_CHANGED(gathered_telemetry.gps_hdop, mavlink_msg_gps_raw_int_get_eph(&msg), HDOP_EX); // hdop * 100 gathered_telemetry.gps_latitude = 0L; gathered_telemetry.gps_longitude = 0l; gathered_telemetry.gps_altitude = 0L; gathered_telemetry.gps_speed = 0L; } break; case MAVLINK_MSG_ID_VFR_HUD: // 74 gathered_telemetry.groundspeed = mavlink_msg_vfr_hud_get_groundspeed(&msg); // 100 = 1m/s gathered_telemetry.heading = mavlink_msg_vfr_hud_get_heading(&msg); // 100 = 100 deg gathered_telemetry.throttle = mavlink_msg_vfr_hud_get_throttle(&msg); // 100 = 100% gathered_telemetry.bar_altitude = mavlink_msg_vfr_hud_get_alt(&msg); // m gathered_telemetry.climb_rate = mavlink_msg_vfr_hud_get_climb(&msg) * 100; // m/s break; case MAVLINK_MSG_ID_STATUSTEXT: mavlink_msg_statustext_get_text(&msg, gathered_telemetry.text); break; case MAVLINK_MSG_ID_ATTITUDE: gathered_telemetry.pitch = mavlink_msg_attitude_get_pitch(&msg) * 180 / M_PI; gathered_telemetry.roll = mavlink_msg_attitude_get_roll(&msg) * 180 / M_PI; break; default: break; } } } }
bool Mavlink::parseMessage(char c) { mavlink_message_t msg; mavlink_status_t status; // allow CLI to be started by hitting enter 3 times, if no heartbeat packets have been received if (mavlink_active == 0 && millis() < 20000 && millis() > 5000) { if (c == '\n' || c == '\r') { crlf_count++; } else { crlf_count = 0; } } //trying to grab msg if(mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) { mavlink_active = 1; switch(msg.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: { mavbeat = 1; apm_mav_system = msg.sysid; apm_mav_component = msg.compid; apm_mav_type = mavlink_msg_heartbeat_get_type(&msg); apmMode = (unsigned int)mavlink_msg_heartbeat_get_custom_mode(&msg); apmBaseMode = mavlink_msg_heartbeat_get_base_mode(&msg); //if(getBit(base_mode,7)) motor_armed = 1; //else motor_armed = 0; //osd_nav_mode = 0; lastMAVBeat = millis(); if(waitingMAVBeats == 1) { enable_mav_request = 1; } return true; break; } case MAVLINK_MSG_ID_SYS_STATUS: { batteryVoltage = (mavlink_msg_sys_status_get_voltage_battery(&msg) / 1000.0f); //Battery voltage, in millivolts (1 = 1 millivolt) current = mavlink_msg_sys_status_get_current_battery(&msg); //Battery current, in 10*milliamperes (1 = 10 milliampere) batteryRemaining = mavlink_msg_sys_status_get_battery_remaining(&msg); //Remaining battery energy: (0%: 0, 100%: 100) return true; break; } case MAVLINK_MSG_ID_GPS_RAW_INT: { latitude = mavlink_msg_gps_raw_int_get_lat(&msg); longitude = mavlink_msg_gps_raw_int_get_lon(&msg); gpsStatus = mavlink_msg_gps_raw_int_get_fix_type(&msg); numberOfSatelites = mavlink_msg_gps_raw_int_get_satellites_visible(&msg); gpsHdop = mavlink_msg_gps_raw_int_get_eph(&msg); gpsAltitude = mavlink_msg_gps_raw_int_get_alt(&msg); gpsCourse = mavlink_msg_gps_raw_int_get_cog(&msg); return true; break; } case MAVLINK_MSG_ID_VFR_HUD: { //osd_airspeed = mavlink_msg_vfr_hud_get_airspeed(&msg); gpsGroundSpeed = mavlink_msg_vfr_hud_get_groundspeed(&msg); course = mavlink_msg_vfr_hud_get_heading(&msg); // 0..360 deg, 0=north throttle = mavlink_msg_vfr_hud_get_throttle(&msg); //if(osd_throttle > 100 && osd_throttle < 150) osd_throttle = 100;//Temporary fix for ArduPlane 2.28 //if(osd_throttle < 0 || osd_throttle > 150) osd_throttle = 0;//Temporary fix for ArduPlane 2.28 altitude = mavlink_msg_vfr_hud_get_alt(&msg); break; } case MAVLINK_MSG_ID_ATTITUDE: { accX = ToDeg(mavlink_msg_attitude_get_pitch(&msg)); accY = ToDeg(mavlink_msg_attitude_get_roll(&msg)); accZ = ToDeg(mavlink_msg_attitude_get_yaw(&msg)); break; } case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: { //nav_roll = mavlink_msg_nav_controller_output_get_nav_roll(&msg); //nav_pitch = mavlink_msg_nav_controller_output_get_nav_pitch(&msg); //nav_bearing = mavlink_msg_nav_controller_output_get_nav_bearing(&msg); //wp_target_bearing = mavlink_msg_nav_controller_output_get_target_bearing(&msg); //wp_dist = mavlink_msg_nav_controller_output_get_wp_dist(&msg); //alt_error = mavlink_msg_nav_controller_output_get_alt_error(&msg); //aspd_error = mavlink_msg_nav_controller_output_get_aspd_error(&msg); //xtrack_error = mavlink_msg_nav_controller_output_get_xtrack_error(&msg); break; } default: //Do nothing break; } } // Update global packet drops counter packet_drops += status.packet_rx_drop_count; parse_error += status.parse_error; return false; }
void mavlink_parse_msg(mavlink_message_t *msg) { switch(msg->msgid) { case MAVLINK_MSG_ID_HEARTBEAT: mavdata.custom_mode = (unsigned char) mavlink_msg_heartbeat_get_custom_mode(msg); mavdata.base_mode = mavlink_msg_heartbeat_get_base_mode(msg); break; case MAVLINK_MSG_ID_SYS_STATUS: mavdata.bat_voltage = mavlink_msg_sys_status_get_voltage_battery(msg) / 1000.0; mavdata.bat_current = mavlink_msg_sys_status_get_current_battery(msg); mavdata.bat_remaining = mavlink_msg_sys_status_get_battery_remaining(msg); break; case MAVLINK_MSG_ID_GPS_RAW_INT: mavdata.gps_altitude = mavlink_msg_gps_raw_int_get_alt(msg) / 1000.0; mavdata.gps_lat = mavlink_msg_gps_raw_int_get_lat(msg) / 10000000.0; mavdata.gps_lon = mavlink_msg_gps_raw_int_get_lon(msg) / 10000000.0; mavdata.gps_fix_type = mavlink_msg_gps_raw_int_get_fix_type(msg); mavdata.gps_nrsats = mavlink_msg_gps_raw_int_get_satellites_visible(msg); mavdata.gps_cog = mavlink_msg_gps_raw_int_get_cog(msg); mavdata.gps_eph = mavlink_msg_gps_raw_int_get_eph(msg); break; case MAVLINK_MSG_ID_VFR_HUD: mavdata.vfr_hud.airspeed = mavlink_msg_vfr_hud_get_airspeed(msg); mavdata.vfr_hud.groundspeed = mavlink_msg_vfr_hud_get_groundspeed(msg); mavdata.vfr_hud.alt = mavlink_msg_vfr_hud_get_alt(msg); mavdata.vfr_hud.climb = mavlink_msg_vfr_hud_get_climb(msg); mavdata.vfr_hud.heading = mavlink_msg_vfr_hud_get_heading(msg); mavdata.vfr_hud.throttle = mavlink_msg_vfr_hud_get_throttle(msg); break; case MAVLINK_MSG_ID_ATTITUDE: mavdata.pitch = RAD2DEG(mavlink_msg_attitude_get_pitch(msg)); mavdata.roll = RAD2DEG(mavlink_msg_attitude_get_roll(msg)); //mavdata.yaw = RAD2DEG(mavlink_msg_attitude_get_yaw(msg)); break; case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: //nav_roll = mavlink_msg_nav_controller_output_get_nav_roll(msg); //nav_pitch = mavlink_msg_nav_controller_output_get_nav_pitch(msg); //nav_bearing = mavlink_msg_nav_controller_output_get_nav_bearing(msg); mavdata.nav_bearing = mavlink_msg_nav_controller_output_get_target_bearing(msg); mavdata.nav_wp_distance = mavlink_msg_nav_controller_output_get_wp_dist(msg); mavdata.nav_alt_error = mavlink_msg_nav_controller_output_get_alt_error(msg); mavdata.nav_aspd_error = mavlink_msg_nav_controller_output_get_aspd_error(msg); mavdata.nav_xtrack_error = mavlink_msg_nav_controller_output_get_xtrack_error(msg); break; case MAVLINK_MSG_ID_MISSION_CURRENT: mavdata.wp_sequence = mavlink_msg_mission_current_get_seq(msg); break; case MAVLINK_MSG_ID_RC_CHANNELS_RAW: mavdata.ch_raw[0] = mavlink_msg_rc_channels_raw_get_chan1_raw(msg); mavdata.ch_raw[1] = mavlink_msg_rc_channels_raw_get_chan2_raw(msg); mavdata.ch_raw[2] = mavlink_msg_rc_channels_raw_get_chan3_raw(msg); mavdata.ch_raw[3] = mavlink_msg_rc_channels_raw_get_chan4_raw(msg); mavdata.ch_raw[4] = mavlink_msg_rc_channels_raw_get_chan5_raw(msg); mavdata.ch_raw[5] = mavlink_msg_rc_channels_raw_get_chan6_raw(msg); mavdata.ch_raw[6] = mavlink_msg_rc_channels_raw_get_chan7_raw(msg); mavdata.ch_raw[7] = mavlink_msg_rc_channels_raw_get_chan8_raw(msg); mavdata.rssi = mavlink_msg_rc_channels_raw_get_rssi(msg); break; case MAVLINK_MSG_ID_WIND: mavdata.wind_direction = (int) mavlink_msg_wind_get_direction(msg); mavdata.wind_speed = mavlink_msg_wind_get_speed(msg); break; case MAVLINK_MSG_ID_SCALED_PRESSURE: mavdata.temperature = mavlink_msg_scaled_pressure_get_temperature(msg); /* 0.01 celsius */ break; case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: //mavdata.relative_alt = mavlink_msg_global_position_int_get_relative_alt(msg) * 0.001; break; default: break; } }